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Executive  Summary 


This  document  summarizes  the  work  performed  under  DARPA  contract  No.  MDA  903-86- K-0037 
during  the  period  October  1985  through  October  1988  at  the  Stanford  Aerospace  Robotics 
Laboratory.  The  research  was  carried  out  by  a  team  of  Ph.D.  candidate  students  under 
the  direction  of  Professor  Robert  H.  Cannon,  Jr.  The  ultimate  goal  of  this  research  is  to 
develop  and  test  a  comprehensive  base  of  new  technology  for  very  quick,  precise  two-arm 
cooperative  manipulation  with  the  use  of  powerful  new  end-point  control  algorithms. 

In  order  to  achieve  this  goal,  the  research  addresses  the  set  of  necessary,  major  ad¬ 
vances  in  robotic  technology,  including:  precision  force  control  and  dynamic  cooperation 
of  two  manipulators. 

Robots  in  industry  and  in  most  research  laboratories  have  position  control  of  gross 
motion,  not  force  control  for  fine  motion  of  mating  parts.  Our  strategy  has  been  to 
study  different  aspects  of  force  control  of  robots.  Some  of  the  tasks  that  have  been 
achieved  include  making  contact  with  objects,  exerting  controlled  forces  on  objects,  and 
making  rapid  motions  of  parts  for  assembly. 

The  obvious  solution  to  end-point  control  of  flexible  manipulators  is  to  measure 
directly  those  quantities  that  are  to  be  controlled;  namely,  to  measure  the  end-point 
position  and  force,  and  to  feed  these  measurements  back  to  a  controller.  Such  a  controller 
would  enable  one  to  use  lighterweight,  flexible,  robots.  There  is,  however,  a  good  reason 
why  this  approach  has  not  been  widely  used  in  the  past:  it  poses  an  extremely  severe 
stability  problem. 

Here,  by  way  of  introduction,  we  describe  how  we  have  proceeded.  Then  we  present 
our  research  results  to  date  in  the  formal  Report  on  Research  that  is  the  body  of  this 
Final  Report. 

Industrial  robots  are  made  stiff  enough  and  strong  enough  to  ignore  loads.  Unavoid¬ 
able  flexibility  in  drive  trains  of  robots,  and  in  their  mounts,  make  precise  end-point 
control  of  flexible  robots  an  issue  of  central  importance.  To  advance  the  technology 
for  force  control  of  manipulators,  and  to  also  contribute  to  lowering  the  cost  of  using 
robots  by  making  them  much  faster  and  more  precise,  we  have  developed  a  series  of 
experimental  systems  that  require  that  an  increasingly  more-advanced  control  capabil¬ 
ity  be  achieved  to  control  them  successfully.  In  each  case,  flexibility  has  been  greatly 
exaggerated  to  force  us  to  solve  the  control  problem  in  a  fundamental  way. 

Supporting  research  described  in  this  report  is  aimed  at  advancing  force  control 
capabilities  in  the  context  of  a  single  one-link  flexible  arm  and  a  single  two-link  arm 
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having  a  very  flexible  drive  train.  Again,  the  goal  is  to  develop  and  test  new  techniques 
for  effective  two-arm  cooperative  manipulation  using  end-point  control  algorithms  for 
manipulators  with  flexible  drive  trains. 

Several  of  our  tasks  have  been  achieved  through  the  use  of  a  mini-manipulator.  The 
mini-manipulator  plays  a  key  role  in  successful  high-performance  force  control.  The 
rationale  for  using  a  mini-manipulator  at  the  tip  of  an  arm  is  that  a  mini-manipulator 
can  be  used  to  perform  tasks  in  a  localized  workspace  at  a  much  higher  bandwidth  than 
is  achievable  by  the  main  arm.  Two  chapters  in  this  report  discuss  force  control  exper¬ 
iments  conducted  with  a  mini-manipulator  attached  to  the  tip  of  a  large  manipulator 
arm. 

We  have  systematically  developed  the  supporting  technologies  for  two-arm  cooper¬ 
ation.  A  crucial  capability  for  two-arm  cooperation  with  flexibility  in  the  drive  train 
is  force  control  at  each  arm  tip,  but  that  is  not  sufficient.  Force  sensing  at  each  joint 
is  also  required  to  ensure  that  the  commanded  joint  torque  is  actually  achieved  at  the 
joint,  with  minimum  phase  loss.  The  hardware  to  implement  two-arm  cooperation  has 
been  designed,  built,  and  is  presently  in  the  experimental  stage.  Larry  Pfeffer  is  leading 
this  research  team. 

Using  the  flexible  arm,  Ray  Kraft  has  recently  demonstrated  position  and  force  con¬ 
trol  with  a  two  dimensional  mini-manipulator  at  the  end  of  the  very  flexible  arm.  Force 
control  techniques  with  the  mini-manipulator  are  thus  significantly  more  complex  than 
the  force  techniques  we  have  previously  developed  with  a  single  degree-of-freedom  force 
sensor.  Control  of  the  extra  degrees  of  freedom  is  not  trivial,  since  the  transformation 
between  joint  velocities  and  end-point  velocities  requires  a  computationally  intensive 
Jacobian. 

Brian  Andersen  is  currently  experimenting  with  a  force  control  methodology  using 
the  mini-manipulator  attached  to  the  end  of  the  two-link  arm.  This  will  be  the  first 
implementation  of  force  control  on  a  non-linear,  multiple  input-multiple  output  plant. 
This  particular  two-link  arm  has  a  flexible  drive  train,  which  increases  the  difficulty  of 
the  problem  by  an  order  of  magnitude. 

The  main  areas  that  are  covered  in  the  report  are  as  follows: 

1.  Cooperating  two-link  arms  with  flexible  drive  trains. 

2.  Control  of  a  flexible  robot  arm  with  a  two- degree-of-freedom  mini-manipulator. 

3.  Force  control  of  a  two-link  arm  with  flexible  drive  train. 

Subsequent  chapters  of  this  report  describe  in  detail  the  progress  in  the  above  areas. 


Chapter  1 

Introduction 


The  Aerospace  Robotics  Laboratory  has  conducted  a  program  of  experimental  research 
that  has  categorically  responded  to  all  aspects  of  the  DARPA  contract  No.  MDA  903-86-K-0037. 
The  goal  of  this  research  is  to  develop  -  and  demonstrate  conclusively  -  key  elements 
in  the  base  of  new  technologies  necessary  for  robots  to  advance  to  a  new,  important 
level  of  capability,  namely,  two-arm  intimate  cooperative  manipulation  that  is  swift, 
highly  dextrous,  precise,  and  sure,  and  that  is  managed  astutely  from  a  high  level  of 
task  specification. 

The  power  and  generality  that  intimate  cooperation  and  deft  movement  will  give  to 
robots  is  quite  vast;  it  allows  the  performance  of  many  new  tasks  previously  entrusted 
only  to  humans.  We  have  spent  the  past  several  years  identifying  critical  enabling 
technologies,  and  have  done  focused  experimental  and  theoretical  work  to  advance  them. 

Specifically,  if  we  are  to  succeed  in  achieving  our  ultimate  goal,  we  must  develop 
and  demonstrate  technology  that  increases  the  utility  of  sets  of  flexible  manipulators 
by  making  them  work  cooperatively  to  handle  tools  and  objects,  especially  elongated 
ones,  accurately  and  with  dispatch,  and  by  providing  and  documenting  key  extensions 
to  basic  control  theory  that  are  necessary  to  accomplishing  such  a  goal. 

Our  research  indicates  that  the  keys  to  deft  and  cooperative  manipulation  are  the 
mastery  of  three  capabilities:  (1)  end-point  sensing  and  pressure  control  of  force  and 
position,  (2)  specification  of  desired  tasks  at  the  object  level,  and  (3)  control  of  quick, 
versatile  end  effectors,  including  mini-manipulators.  For  example,  directly  controlling 
the  endpoint  of  flexible  manipulators  provides  for  a  one-to-one  matching  with  desired 
object  motion  (which  joint-angle  control  does  not). 

The  following  are  specific  control  capabilities,  crucial  to  the  achievement  of  our  goals, 
for  which  we  have  developed  algorithms  and  tested  them  experimentally: 

1.  Authoritative  end-point  control,  both  position  and  force. 

2.  Programmable  compliance  of  the  end  effector. 

3.  Compensation  for  nonlinearities  in  the  system’s  dynamics,  and  robustness  to  pa¬ 
rameter  variation,  e.g.,  payload  mass  (including  adaptive  control). 
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CHAPTER  1.  INTRODUCTION 


4.  Basic  strategies  for  cooperation,  including  hierarchically  structured  task  parti¬ 
tioning,  control  mode  switching,  and  the  astute  management  of  redundant  sensor 
information. 

Our  main  emphasis  is  on  the  design  and  demonstration  of  these  new  control  al¬ 
gorithms  for  the  cooperative  operation  of  multiple  manipulators.  Techniques  that  are 
generic  to  control  of  multiple  flexible  manipulators  performing  complicated  tasks  must 
be  developed  in  such  a  way  that  others  can  then  use  them  to  achieve  similar  goals. 

In  a  broader  sense,  the  nature  of  our  goals  for  a  new  level  of  robot  capabilities  has 
lead  us  to  identify  a  number  of  advances  that  must  be  contributed  to  the  body  of  basic, 
generic  automatic  control  theory;  and  we  have  made  a  series  of  such  contributions. 

In  achieving  our  research  objective  we  intend  to  solve  fundamental  problems  in  the 
field  of  flexible  manipulator  control  separately  from  the  (equally  important)  problems  of 
three  dimensional  geometry,  gravity,  complicated  sensor  systems,  and  sophisticated  end 
effectors.  This  will  be  possible  by  operating,  in  these  experiments,  in  a  horizontal  plane, 
and  by  using  upgraded  versions  of  the  special  sensors  we  have  developed  as  surrogates 
for  more  sophisticated  ones. 

We  must  be  able  to  take  advantage  of  the  flexibility  in  the  manipulators  to  allow 
rapid  movements  without  generating  excessive  contact  forces,  and  we  must  demonstrate 
its  value  to  cooperative  tasks  in  which  there  is  serious  kinematic  overconstraint. 

The  strategy  we  have  used  for  achieving  the  set  of  enabling  technology  goals  is  to 
pursue  a  sequence  of  specific  experimental  demonstrations.  Each  demonstration  was 
conceived  to  require  a  particular  technology  advance,  and  the  set  of  them  to  enable  the 
graceful,  intense  cooperation  of  two  flexible  arms. 


1.1  Background 

Current  work  achieved  under  this  contract  is  explained  in  detail  in  the  body  of  this 
report.  The  research  centers  around  the  following  three  major  topics: 

Creating  an  advanced  test  bed  for,  and  achieving  control  of  intimately  cooperating 
two-link  arms  with  flexible  drive  trains  (Chapter  2). 

Achieving  very  quick,  precise  force  and  position  control  of  a  flexible  robot  arm  with 
a  two-degree-of- freedom  mini-manipulator  (Chapter  3). 

Achieving  deft  force  control  of  a  two-link  arm  with  a  flexible  drive  train  and  a  two- 
degree-of-freedom  mini-manipulator  (Chapter  4). 

The  work  which  was  drawn  on  to  achieve  the  results  in  the  body  of  this  report  follows 
most  directly  from  the  DARPA  sponsored  groundwork  in  previous  contracts.  We  have 
also  carried  out  some  other  work,  sponsored  by  the  AFOSR  and  NASA,  in  flexible  two- 
link  arm  control  and  on  spacecraft  manipulators.  The  direct  benefit  of  carrying  out 
this  related  work,  in  the  same  lab,  is  the  ability  to  easily  drawn  upon  it.  This  applies 
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not  only  to  control  algorithms,  but  also  to  software,  computer  resources,  sensors,  and 
in-house  hardware  designs. 

As  a  sampling  of  recent  experimental  results  that  are  germane,  we  have  achieved: 

1.  The  first  end-point  control  of  a  very  flexible  manipulator. 

2.  The  first  real-time  adaptive  control  of  a  very  flexible  beam. 

3.  The  first  experiments  with  potential-field  guidance  of  manipulation. 

4.  The  first  switching,  from  (optical)  position  control  to  force  control  of  a  flexible 
manipulator  contacting  a  moving  object. 

5.  The  first  impedance-based  hierarchical  control  of  two  cooperating  two-link  arms, 
in  which  the  operator’s  commands  are  for  object  motion  directly. 

6.  The  first  end-point  control  of  a  manipulator  operating  from  a  completely  freely 
moving  base. 

The  research  in  the  Stamford  Aerospace  Robotics  Lab  has  laid  the  foundation  for 
the  investigation  of  noncolocated  control  of  flexible  structures  and  end-point  sensing. 

We  have  addressed  the  basic  question  of  what  must  be  done  to  guarantee  stability 
and  robustness  (insensitivity  to  large  variations  in  plant  parameters)  in  controllers  using 
noncolocated  sensors  and  actuators:  amd  what  are  the  ultimate  limits  to  what  can  be 
achieved  on  a  very  flexible  manipulator.  Experiments  on  a  system  of  four  inertia  disks 
connected  by  a  torsion  rod  with  0.3%  damping  demonstrated  the  ease  of  achieving 
colocated  control  (due  to  alternating  poles  amd  zeros  on  the  imaginary  axis)  and  the 
difficulty  of  achieving  stable  and  robust  noncolocated  control.  LQG  controllers  could 
always  be  found  to  notch  out  resonant  modes  and  stabilize  a  noncolocated  system; 
but  the  controllers  were  extremely  sensitive  to  variations  in  those  resonant  frequencies 
caused  by  changes  in  plamt  parameters. 


Initial  experiments  in  noncolocated  control  of  very  flexible  (0.5Hz)  manipulators 
were  conducted  by  Eric  Schmitz  under  NASA  funding.  Rigorous  mathematical  models 
of  the  flexible  manipulator  were  developed  to  explain  the  dynamics  of  the  structure. 
The  fundamental  response  time  limit  in  his  single-link  very  flexible  arm  is  associated 
with  the  bending  wave  propagation  speed.  Optical  end-point  sensing  was  used  to  locate 
a  light  at  the  arm’s  tip,  and  stable  end-point  position  control  w as  established.  The 
closed-loop  bandwidth  was  twice  as  high  as  the  arm’s  first  cantilevered  frequency,  which 
is  the  performance  limit  on  the  noncolocated  control  system. 

Jim  Maples  extended  this  work  under  DARPA  funding  to  stable  force  control  at 
the  manipulator’s  tip.  A  new  end-point  force  sensor  was  developed  to  allow  for  the 
demonstration  of  a  slew-and-touch  maneuver  to  a  moving  target  without  pause  in  the 
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arm’s  motion.  Furthermore,  very  fast  localized  response  has  been  achieved  by  adding  a 
short  rigid  wrist  to  the  end  of  a  flexible  arm.  The  rational  was  that  many  operations 
require  fast,  precise  movement  within  a  small  workspace  and  relatively  slower  gross 
movement  over  larger  distances.  This  was  the  beginning  of  noncolocated  control  of 
truly  multiple-input  multiple-output  systems. 


A  much  more  complicated  two-link  manipulator  system  has  been  designed,  built, 
and  controlled  by  Mike  Hollars  under  DARPA  and  AFOSR  funding.  This  two-link  arm 
has  rigid  members  with  lumped  flexibility  in  the  form  of  elastic  linear  springs.  The 
two-dimensional  geometry  gives  rise  to  highly  nonlinear  equations  of  motion  which  are 
more  representative  of  modern  industrial  robots.  Not  only  has  stable  colocated  closed 
loop  control  been  experimentally  achieved,  but  also  stable  noncolocated  control.  The 
noncolocated  controller  uses  a  constant  gain  extended  Kalman  filter  to  estimate  the 
state  of  the  two-link  arm. 


Stan  Schneider,  working  under  NASA  funding,  has  completed  work  on  a  fixed- 
base  cooperative  manipulation  experiment  where  the  problem  of  utilizing  multiple  rigid 
robotic  arms  to  control  a  single  object  was  examined.  This  research  had  three  major 
goals: 


1.  Experimental  evaluation  of  fundamental  cooperative  dynamic  control  techniques. 

2.  Investigation  of  manipulation  strategies,  especially  as  they  pertain  to  multiply- 
armed  robotic  systems. 

3.  Achieving  a  heirarchical  control  system  in  which,  for  the  first  time,  the  operator’s 
commands  are  issued  at  the  object  level. 


In  addition  to  cooperative  dynamic  control,  the  experimental  system  incorporated 
real-time  vision  feedback,  a  novel  programming  technique,  and  a  graphical  high-level 
user  interface.  Not  only  these  subsystems,  but  also  their  interfaces  and  interactions 
have  been  studied,  and  a  total,  optimized  system  achieved. 


We  have  systematically  developed  the  supporting  technologies  for  flexible  two-arm 
cooperation.  A  crucial  capability  for  two-arm  cooperation  with  flexibility  in  the  drive 
train  is  force  control  at  each  arm  tip,  but  that  is  not  sufficient.  Force  sensing  at  each 
joint  is  also  required  to  ensure  that  the  commanded  joint  torque  is  actually  achieved  at 
the  joint,  with  minimum  phase  loss.  The  hardware  to  implement  two-arm  cooperation 
has  been  designed,  built,  and  is  presently  in  the  experimental  stage. 


1.2.  FUTURE  PLANS 
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1.2  Future  Plans 

Our  future  research  plans  include  making  contributions  of  three  kinds  that  are  central 
to  advancing  robot  capability  from  its  present  level  to  the  much  wider  realm  of  use  for 
which  it  is  inherently  competent: 

1.  We  will  make  key  new  advances  in  the  specific  technologies  that  are  critical  to 
achieving  true,  effective  integrated  cooperation  between  compliant  manipulators; 

2.  We  will  advance  the  art  of  motion  planning  and  execution,  for  both  gross  and  fine 
maneuvering  of  awkward  objects  amidst  obstacles,  so  that  users  can  specify  object 
motions  at  a  high  level;  and 

3.  We  will  unite  the  path  planning  and  manipulation  functions  through  a  major  fun¬ 
damental  joint  effort  -  with  the  Stanford  Computer  Science  Robotics  Laboratory 
-  in  the  foundation  and  achievement  of  comprehensive  vertical  integration. 

Specifically,  we  will  demonstrate  experimentally  the  intimate  vertical  integration  of  path 
planning  with  the  manipulative  skills  of  a  pair  of  deft,  cooperating  robotic  arms  in  the 
rapid  assembly  of  a  complex  configuration  of  awkward  objects,  all  in  response  to  only 
high-level  instructions. 

1.3  Summary 

Within  this  report  three  major  topics  are  addressed. 

Chapter  2  discusses  cooperating  two-link  arms  with  flexible  drive  trains.  The  basic 
objective  of  this  research  is  to  develop  and  demonstrate  cooperative  control  of  flexible 
robots.  The  extension  of  robots  to  tasks  that  are  not  readily  accomplished  with  one 
(robot)  hand  has  distinct  advantages:  cooperating  robots  can  handle  a  broader  class 
of  tasks,  and  they  may  be  accomplished  faster,  more  accurately,  and  in  a  more  loosely 
structured  environment. 

For  generality,  we  wanted  a  pair  of  6  DOF  manipulators  with  a  large  dexterous 
workspace.  Furthermore,  we  wanted  to  be  able  to  change  the  way  the  workspaces 
overlapped  for  different  experiments.  However,  we  wanted  to  avoid  the  time  and  cost  of 
working  against  gravity  in  the  long  primary  links  and  to  have  a  simple  means  to  lessen 
the  chance  of  collision  (as  collision  avoidance  a  primary  objective  for  other  research). 
This  lead  us  to  the  SCARA  configuration,  with  the  arms  side  by  side,  one  left  handed 
and  one  right  handed. 

The  SCARA  configuration  keeps  the  first  two  links  (those  with  the  greatest  mass 
and  moment  arms)  in  the  horizontal  plane.  This  configuration  also  allows  us  to  begin 
cooperative  work  with  less  chance  of  collisions,  as  the  robots  generally  have  their  elbows 
well  away  from  each  other.  This  strategy  allows  us  to  avoid  arm  collisions  by  monitoring 
endpoint  position  primarily.  The  kinematics  are  also  considerably  simplified  by  the 
design’s  four  parallel  axes.  In  our  design,  the  actuators  for  the  first  two  links  remain 
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fixed  to  the  base  (as  in  the  Adept  I  robot)  so  that  the  mass  of  those  actuators  never 
moves.  Similarly,  we  use  brushless  motors,  and  low  gear  ratios  for  these  unmoving 
actuators,  so  that  we  have  low  levels  of  coulomb  friction  in  the  degrees  of  freedom  that 
have  the  flexibility  and  the  primary  responsibility  during  cooperation.  For  the  rest  of 
the  degrees  of  freedom,  we  accept  the  need  for  higher  gearing  using  small  servomotors 
with  brushes. 


Chapters  3  and  4  discuss  the  use  of  a  mini-manipulator.  The  mini-manipulator 
plays  a  key  role  in  successful  high-performance  control.  The  rationale  for  using  a  mini¬ 
manipulator  at  the  tip  of  an  arm  is  that  a  mini- manipulator  can  be  used  to  perform 
tasks  in  a  localized  workspace  at  a  much  higher  bandwidth  than  is  achievable  by  the 
main  arm.  By  using  end-point  control  in  conjunction  with  a  mini-manipulator,  a  flexible 
robot  would  be  able  to  achieve  high  bandwidth,  precise  control  over  a  large  workspace. 
It  is  these  considerations  which  motivated  the  research  on  position  and  force  control  of 
a  flexible  robot  arms  with  a  two-degree-of- freedom  mini-manipulator. 

Chapter  3  discusses  control  of  a  flexible  robot  arm  with  a  two-degree-of-freedom 
mini-manipulator.  The  flexible  arm  has  distributed  flexibility  and  was  intentionally 
designed  to  exaggerate  structural  flexibility  in  the  horizontal  plane  while  remaining  stiff 
in  the  gravity  influenced  vertical  direction.  The  purpose  of  exaggerating  the  horizontal 
flexibility  in  this  system  was  two  fold:  (1)  to  make  problems  associated  with  flexibility 
more  readily  apparent,  and  (2)  to  lower  the  resonant  frequencies  of  the  flexible  arm  into 
a  region  where  easy  laboratory  sampling  rates,  in  the  neighborhood  of  25  to  100  Hz, 
could  be  used.  A  potentiometer  is  collocated  with  the  hub  motor  and  is  used  to  measure 
the  base  angle  of  the  flexible  arm.  A  light  emitting  diode  (LED)  array,  whose  position 
can  be  sensed  by  a  photodetector,  is  located  at  the  end-point  of  the  flexible  arm. 

The  mini-manipulator  is  attached  to  the  end-point  of  the  flexible  arm  along  one  of 
the  side-rails.  Essentially,  it  is  a  five  link,  closed  kinematic  chain  —  the  base  link  being 
fixed  to  the  flexible  arm.  Each  of  the  two  inboard  links  is  rigidly  attached  to  one  of 
the  two  motors  that  drive  the  mini-manipulator.  Located  at  the  joint  between  the  two 
outboard  links  is  another  LED  array  which  is  used  to  sense  the  position  of  the  end-point 
of  the  mini-manipulator. 

Below  this  LED  array  is  a  vertical,  force  sensing,  aluminum  beam.  It  is  approxi¬ 
mately  9.5  cm  long  and  is  equipped  with  strain  gauges  which  provide  a  means  of  mea¬ 
suring  end-point  force.  At  the  bottom  end  of  the  force  beam  is  a  circular  contact  roller, 
which  allows  the  mini-manipulator  to  exert  only  normal  forces  on  objects  it  comes  into 
contact  with. 


Finally,  Chapter  4  describes  work  that  has  been  accomplished  to  date  on  the  two-link 
arm  with  a  flexible  tendon  drive  and  mini-manipulator.  The  objective  of  this  research 
is  to  allow  the  arm  to  slew  into  contact  with  a  target,  make  a  smooth  touchdown,  and 
maintain  a  constant  force  on  the  target,  using  the  mini-manipulator  for  augmentation 


1.3.  SUMMARY 


9 


of  precision  control.  We  believe  the  mini-manipulator  is  going  to  play  a  key  role  in 
successful  high-performance  force  control. 

During  the  time  covered  by  this  report  we  have  demonstrated  initial  position  and 
force  control  with  the  mini-manipulator  on  a  fixed  base.  This  work  will  form  the  basis 
of  the  control  algorithms  when  the  mini-manipulator  is  added  to  the  two-link  arm. 

The  capability  we  wish  to  develop  is  very-high-bandwidth,  precise  control  of  the 
force  at  the  tip  of  a  two-link  manipulator  having  a  very  flexible  drive  train.  We  plan 
to  demonstrate  this  capability  by  (1)  performing  rapid  slew  and  touch  with  a  fixed 
target  while  having  no  overshoot  in  the  force!  (2)  controlling  the  arm  so  that  its  tip 
moves  along  a  wavy  surface  while  maintaining  a  constant  force  on  the  surface,  and  (3) 
slewing  into  contact  with  a  moving  target  and  maintaining  a  constant  force  on  it  while 
it  continues  to  move. 

The  chapters  which  follow,  then,  present  the  specific  set  of  (we  submit)  crucial 


contributions  that  DARPA  contract  No.  MDA  903-86-K-0037  has  allowed  us  to  make 
to  a  new  technology  base  that  will  substantially  advance  the  new  capabilities  and  value 
of  the  next  generation  of  robotic  systems. 


Chapter  2 


Hardware  Development, 
Modeling,  and  Experimental 
Control  of  Two  Cooperating, 
Flexible-Drive  Manipulators 


Lawrence  E.  Pfeffer 


Introduction 

la  this  chapter,  we  report  the  progress  and  results  from  three  years  of  experimental 
research  on  the  cooperating,  flexible-drive  manipulator  testbed  in  our  laboratory.  This 
testbed  provides  a  base  for  the  development,  for  the  first  time,  of  the  following  underlying 
technologies:  hierarchical,  cooperative  control;  concurrent  mechanism/control  design; 
realtime  multiprocessor  control  of  robots;  advanced  sensing/perception;  and  graphical 
meta-teleoperative  user  interfaces.  These  are  technologies  that  are  essential  for  the  new 
level  of  robot  capabilities  and  missions  that  our  country  needs  for  a  strong  defense 
system  and  a  vigorous  manufacturing  economy. 

The  work  reported  here  was  performed  during  the  last  year  of  DARPA  contract 
MDA903-86-K-0037.  We  preceed  it  in  section  2.1  with  an  overview  of  the  work  reported 
previously.  Then  we  develop  (beginning  in  section  2.2)  the  complete  kinematic  model: 
rigorous  coordinate  frame  definitions,  full  forward  kinematics,  the  inverse  kinematics, 
and  the  Jacobian  matrix.  The  impact  of  the  kinematics’  structure  on  control  algorithms 
is  discussed,  including  computational  complexity  and  effects  of  singularities.  The  next 
section,  2.3  presents  the  results  from  the  identification  and  control  experiments  per¬ 
formed  under  this  contract:  mass  property  identifications,  scale  factor  measurements, 
and  describing  function  measurements  of  the  testbed.  The  final  section,  2.4  describes 
the  follow-on  research  that  will  be  performed  on  this  testbed  under  a  new  DARPA 
contract. 
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Figure  2.1:  The  Cooperating  Flexible  Manipulators  Holding  a  Long  Part 

2.1  Summary  of  Previous  Research  Progress 

Over  the  past  three  years,  we  have  designed,  built,  and  started  experimental  research 
on  the  cooperating  flexible  manipulators  testbed.  This  research  facility  is  comprised 
of  a  pair  of  manipulators  designed  for  cooperation;  a  fast  and  flexible  realtime  con¬ 
trol  computer;  a  SUN  workstation  for  user  interaction  and  code  development;  and  the 
sensor/actuator/safety  interface  subsystems  that  connect  the  realtime  computer  to  the 
robots.  Each  phase  of  the  design,  fabrication,  and  experimentation  has  yielded  valuable 
ideas  in  the  theory  and  practice  of  robotics.  This  section  is  an  overview,  intended  to 
provide  a  context  for  the  rest  of  the  report. 

Figure  2.1  shows  the  two  cooperating  manipulators  (posed  handling  a  long  part.) 
There  are  several  aspects  of  the  manipulator  design  that  are  worth  emphasizing: 

•  Each  arm  is  an  identical  six  degree-of- freedom  manipulator,  and  therefore  can  both 
position  and  orient  its  end  effector  within  its  dexterous  workspace  -  a  necessary 
geometric  freedom  for  generic  experiments  in  cooperation. 

•  The  manipulators  are  of  the  basic  SCARA  configuration,  with  their  first  two 
links  moving  in  a  horizontal  plane,  and  the  third  link  a  vertical  linear  stage.  This 
configuration  has  two  basic  advantages:  first,  it  eliminates  large  gravity  torques  on 
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the  primary  actuators;  second,  having  the  first  four  axes  parallel  greatly  simplifiies 
the  kinematics. 

•  The  SCARA  configuration  provides  a  wide,  fiat  dextrous  workspace  for  each  arm. 
This  aids  in  task  planning  and  demonstrations,  as  it  is  similar  to  the  sort  of 
two  handed  manipulation  that  a  person  (or  two  people)  might  do  at  a  desk  or 
workbench. 

•  The  robots  are  each  attached  to  individual  mounts  that  are  stiff,  massive,  yet 
movable.  This  permits  free  experimentation  with  varying  degrees  of  workspace 
overlap  and  with  different  robot  orientations,  e.g.  side  by  side,  across  from  each 
other,  or  at  right  angles. 

•  The  first  two  degrees  of  freedom  (D.O.F.)  of  each  arm  have  lumped  (as  opposed 
to  distributed)  flexibility  built  into  their  drive  systems.  These  are  the  D.O.F. 
that  correspond  to  the  large  motions  (and  large  velocities)  that  span  the  mutual 
workspace.  This  flexibility,  properly  controlled,  permits  cooperative  control  that 
is  precise  and  gentle  without  requiring  a  perfectly  known  and  structured  environ¬ 
ment. 

•  There  are  sensors  for  each  actuator,  and  more  importantly,  endpoint  and  interme¬ 
diate  joint  sensors  to  measure  the  quantities  that  matter  most  in  cooperation. 

The  basic  objective  of  this  research  is  to  develop  and  demonstrate  cooperative  control 
of  flexible  robots.  The  extension  of  robots  to  tasks  that  are  not  readily  accomplished 
with  one  (robot)  hand  has  distinct  advantages:  cooperating  robots  can  handle  a  broader 
class  of  tasks,  and  they  may  be  accomplished  faster,  more  accurately,  and  in  a  more 
loosely  structured  environment.  The  background,  motivation  and  objectives  are  detailed 
in  last  year’s  report.  The  effects  of  the  project  objectives  on  the  design  are  discussed 
next. 


2.1.1  Design  Philosophy 

The  design  philosophy  for  this  project  has  been  to  develop  a  versatile  testbed  that  is 
easy  to  experiment  with,  and  at  the  same  time  to  minimize  complications  not  in  the 
mainstream  of  our  research. 

For  generality,  we  wanted  a  pair  of  6  DOF  manipulators  with  a  large  dexterous 
workspace.  Furthermore,  we  wanted  to  be  able  to  change  the  way  the  workspaces 
overlapped  for  different  experiments.  However,  we  wanted  to  avoid  the  time  and  cost  of 
working  against  gravity  in  the  long  primary  links  and  to  have  a  simple  means  to  lessen 
the  chance  of  collision,  as  collision  avoidance  is  not  one  of  our  primary  objectives.  This 
lead  us  to  the  SCARA  configuration,  with  the  arms  side  by  side,  one  left  handed  and 
one  right  handed. 

The  SCARA  configuration  keeps  the  first  two  links  (those  with  the  greatest  mass 
and  moment  arms)  in  the  horizontal  plane.  This  configuration  also  allows  us  to  begin 
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cooperative  work  with  less  chance  of  collisions,  as  the  robots  generally  have  their  elbows 
well  away  from  the  other.  This  strategy  allows  us  to  avoid  collisions  by  monitoring 
endpoint  position  primarily.  The  kinematics  are  also  considerably  simplified  by  the 
design’s  four  parallel  axes.  In  our  design,  the  actuators  for  the  first  two  links  remain 
fixed  to  the  base  (as  in  the  Adept  I  robot)  so  that  the  mass  of  those  actuators  never 
moves.  Similarly,  we  use  brushless  motors  and  low  gear  ratios  for  these  unmoving 
actuators,  so  that  we  have  low  levels  of  coulomb  friction  in  the  degrees  of  freedom  that 
have  the  flexibility  and  the  primary  responsibility  during  cooperation.  For  the  rest  of 
the  degrees  of  freedom,  we  accept  the  need  for  higher  gearing  and  small  servomotors 
with  brushes. 

The  robots  are  extremely  well  instrumented.  There  are  position  sensors  on  each 
actuator  (digital  shaft  encoders),  which  are  supplemented  by  resolvers  on  the  primary 
motors  (for  the  first  two  DOF)  that  yield  analog  position  and  rate.  The  two  primary 
joints,  separated  from  their  actuators  by  the  flexibility,  are  instrumented  with  higher 
resolution  encoders  for  endpoint  sensing.  Force  and  torques  are  measured  by  joint  torque 
sensors  built  into  the  first  two  joints,  as  well  as  by  endpoint  force/torque  sensors  (not 
shown  mounted  in  photograph.)  These  sensors  make  it  possible  to  obtain  an  accurate 
and  timely  estimate  of  both  the  actuator  and  endpoint  quantities  of  interest.  This  is  a 
key  to  good  control  in  general  and  cooperation  in  particular. 

2.1.2  Computer  system 

The  realtime  computer  for  the  project  is  a  multiple  processor  system  coupled  to  the 
rest  of  the  lab’s  computing  resources  over  the  ethernet.  The  realtime  system  is  capa¬ 
ble  of  rapid  computation  of  complex  control  algorithms,  hierarchical  control,  modular 
expansion  or  upgrading,  as  well  as  ease  of  experimentation  and  data  collection. 

The  overall  computer  consists  of  processors  that  are  tied  together  through  a  high¬ 
speed  communications  systems.  The  development  system,  or  host,  is  the  computer 
that  the  user  interacts  directly  with.  Ours  is  a  SUN-3/ 140  workstation.  The  realtime 
control  system  was  developed  in  our  laboratory  as  a  platform  for  controls  research.  The 
realtime  system  is  a  multiple  processor  computer,  composed  of  single  board  processors 
and  peripheral  devices  for  communications  with  the  robots,  the  host  computer,  and 
other  experimental  equipment.  One  of  the  key  aspects  to  the  power  and  utility  of  this 
system  is  that  both  the  host  and  the  realtime  system  use  the  same  model  microprocessor, 
and  the  same  model  of  electrical  backplane  (bus).  This  allows  us  to  develop  and  test 
software  for  the  realtime  system  using  all  the  tools  available  on  the  host. 

The  processor  boards  for  the  realtime  system  are  MVME  147  and  MVME133  single 
board  processors  manufactured  by  Motorola  Inc.,  microcomputer  division.  These  cuds 
run  at  16  -24  MHz,  and  each  includes  a  32  bit  microprocessor  (68030  or  68020,  respec¬ 
tively),  a  hardware  floating  point  co-processor  (68882  or  68881),  one  to  four  megabytes 
of  memory,  as  well  as  timers,  a  serial  port,  and  sockets  for  ROM,  etc.  In  terms  of  floating 
point  calculation  speed,  (which  is  the  major  bottleneck  in  robotics/controls  work)  one 
of  these  cards  is  substantially  faster  than  a  VAX-11/780,  especially  on  transcendental 
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functions  that  are  common  time  sinks  (e.g.  trig,  arctrig,  and  square  root)  in  robotics. 

The  current  realtime  system  consists  of  two  processor  cards  (with  the  possibility  of 
adding  more),  an  analog  to  digital  converter  board  (for  reading  analog  signals),  digital 
to  analog  boards  (for  commanding  actuators),  a  digital  input/output  board  (for  read¬ 
ing/writing  digital  signals,  e.g.,  reading  shaft  encoder  inputs,  or  writing  enable  bits  to 
the  safety  system.) 


This  overview  has  described,  in  general  terms,  the  experimental  testbed  that  we  have 
built,  analysed,  and  begun  experimenting  with  over  the  last  three  years.  The  next 
sections  describe,  in  greater  detail,  the  final  year’s  research  using  the  testbed. 


2.2  Kinematic  Model 

In  order  to  build  up  a  usable  model  of  the  robot  system  (beginning  here  with  the  robots’ 
kinematics),  we  need  to  have  a  set  of  well  defined  coordinate  frames.  Any  ambiguity  or 
mistakes  at  this  stage  will  ripple  through  the  entire  model  building  process,  rendering 
it  useless.  Thus,  we  have  to  be  precise  about  our  model’s  foundations.  There  are  two 
types  of  coordinate  systems  that  we  will  be  interested  in,  these  are: 

Cartesian:  These  are  coordinate  systems  that  use  three  mutually  perpendicular 
(right  handed  throughout  this  discussion)  basis  vectors  and  an  origin  to  define  a  coor¬ 
dinate  frame  from  which  positions  and  orientations  are  measured.  Positions  are  defined 
by  taking  components  of  the  position  vector  parallel  to  each  of  the  basis  vectors.  Orien¬ 
tations  are  described  by  a  series  of  roll-pitch-yaw  rotations  ( 1-2- 1  body  fixed  rotations, 
true  Euler  angles)  about  axes  that  begin  aligned  with  the  reference  frame.  Our  primary 
interest  is  in  the  robot’s  endpoint/toolpcint  position  measured  in  the  robot’s  base  frame 
[2,  page  88].  Intermediate  frames  are  introduced,  as  well,  for  steps  in  the  analysis,  these 
are  attached  to  each  link  of  the  manipulator. 

Relative-displacement:  These  are  coordinate  systems  that  describe  the  configuration 
of  the  robot  in  terms  of  (linear  or  angular)  displacements  of  parts  of  the  mechanism 
relative  to  one  another.  We  will  be  primarily  interested  in  joint  space  and  actuator 
space.  The  joint  space  coordinates  of  the  robot  are  defined  as  an  ordered  vector  of 
the  robot’s  joint  displacements  (our  link  frames  and  joint  coordinates  were  described  in 
last  year’s  annual  report  [1].  The  actuator  space  coordinates  are  an  ordered  vector  of 
actuator  displacements,  in  our  case,  these  are  all  radian  measures  of  the  motors  relative 
to  the  reference  configuration. 

2. 2.0.1  Notation  for  Coordinates  of  Interest 

Elements  in  a  vector  that  denotes  robot  configuration  (position  and  orientation)  are 
called  q’s.  This  is  in  keeping  with  the  nomenclature  used  in  the  dynamics  methodology 
that  we  are  using,  and  avoids  the  semantic  violence  of  refering  to  a  linear  displacement 
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as,  for  example,  #3,  when  in  reality  the  quantity  is  really  a  linear  displacement.  Following 
the  q,  is  another  letter  that  defines  what  kind  of  frame  we  are  dealing  with:  We  use  c  (for 
base  frame  Cartesian),  j  (for  joint  space),  and  m  (for  motor  space).  Finally,  we  append 
a  subscript  to  denote  which  coordinate  in  the  vector  we  are  referring  to,  this  subscript 
can  be  numeric  (1  to  7)  or  a  letter  mnemonic  that  is  meaningful  in  the  coordinate  space 
we  are  using.  The  definitions  following  give  examples. 

For  the  cartesian  coordinate  systems,  we  adopt  the  following  format  and  nomen¬ 
clature  for  vectors  and  their  components.  The  cartesian  space  configuration  vector  is 
referred  to  as  qc;  the  individual  components  of  qc  are  defined  below  in  their  order. 


X  position 
Y  position 
Z  position 
yaw  angle 
pitch  angle 
roll  angle 

Gripper  jaw  opening 


qci  or  qcx 
qc2  or  qcy 
qc3  or  qcz 
qc4  or  qcy 
qcs  or  qcp 
qce  or  qcr 
qc7  or  qcg 


where  positions  are  in  meters  and  angles  are  in  radians. 

For  (robot)  joint  space,  the  entire  configuration  vector  is  referred  to  as  qj  the  vector 


fonnat  is: 

Joint  1  (shoulder)  angle 

qj\ 

or 

qj . 

Joint  2  (elbow)  angle 

qj* 

or 

qj' 

Joint  3  (vertical  slide)  position 

qj* 

or 

qjz 

Joint  4  (yaw)  angle 

qj* 

or 

qjy 

Joint  5  (pitch)  angle 

qh 

or 

qjv 

Joint  6  (roll)  angle 

qje 

or 

qjr 

Joint  7  (gripper)  opening  distance 

qj 7 

or 

qj8 

where  positions  are  in  meters  and  angles  are  in  radians.  The  joint  coordinates  follow 
the  convention  used  by  [2]  angles  are  measured  from  the  positive  X  axis  of  the  previous 
frame  to  the  positive  X  axis  of  the  current  one,  i.e.  qj\  is  the  angle  between  +Xo  and 
+Xi.  For  the  linear  joints,  qj$  denotes  the  distance  between  the  origins  of  frames  3  and 
4;  qjr  denotes  the  distance  between  the  gripper  jaws. 

For  actuator  space,  the  entire  configuration  vector  is  referred  to  as  qm  the  vector 


format  is: 

Motor  1  (shoulder)  angle 

qm\  or  qm. 

Motor  2  (elbow)  angle 

qm?  or  qme 

Motor  3  (vertical  slide)  angle 

qm 3  or  qmx 

Motor  4  (yaw)  angle 

qm 4  or  qmv 

Motor  5  (pitch)  angle 

qm$  or  qmv 

Motor  6  (roll)  angle 

qm 6  or  qmr 

Motor  7  (gripper  slide)  angle 

qm7  or  qmg 
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where  angles  are  in  radians. 

The  wrist  mechanism  we  are  using  couples  motors  4  through  6  into  orientation  in 
a  complex  (meaning  not  one  to  one,  except  for  yaw)  manner.  Our  transmission,  in 
fact,  also  couples  both  of  the  first  two  motors  into  the  elbow  joint’s  motion,  but  the 
identification  of  shoulder  and  elbow  motors  is  still  clear,  as  the  coupling  is  one  way  (only 
the  shoulder  motor  affects  the  shoulder  joint,  the  other  of  the  two  is  called  the  elbow 
motor).  The  wrist  coupling  is  less  straightforward.  The  fourth  joint  coordinate,  qj4 
depends  only  on  one  of  the  motor  coordinates,  namely  qm4.  However,  the  next  two  joint 
coordinates,  qj$  and  qj$  depend  on  combinations  of  motor  coordinates.  Occasionally,  we 
will  refer  to  motors  4,  5,  and  6,  as  the  yaw,  pitch,  and  roll  motors.  This  is  an  arbitrary 
mnemonic  device.  It’s  origin  is  from  visualizing  being  a  pilot  sitting  in  the  tool,  facing 
out  along  the  tool’s  axis  (opposite  the  Ze  vector).  From  this  point  of  view,  one  can 
think  of  roll,  pitch,  and  yaw  exactly  as  motions  of  qje  =  qjr,  qjs  =  qjr,  and  qj4  =  qjv , 
respectively.  Since  qjy  depends  only  on  qm4t  this  motor  can  be  referred  to  as  the  yaw 
motor,  and  we  will  occasionally  refer  to  motors  5  and  6  as  pitch  and  roll,  even  though 
this  is  arbitrary  nomenclature  it  is  occasionally  useful. 

2.2.0.2  Link  Frames  and  Associated  Transforms 

We  now  present  the  definitions  of  the  coordinate  frames  used  in  deriving  the  forward 
kinematics  of  one  of  our  cooperating  manipulators.  We  will  follow  the  convention  of 
[2],  and  will  use  homogeneous  transforms  as  defined  in  his  chapter  2  to  define  the 
transforms  between  each  adjacent  frame,  and  finally  the  full  kinematic  transform  for 
one  of  our  manipulators.  The  relative  simplicity  of  our  robots’  kinematics  will  then  be 
apparent,  and  will  be  addressed.  Since  the  possible  frame  assignments  for  a  manipulator 
are  not  unique,  a  brief  description  of  the  origin  and  orientation  of  each  frame  we  use  is 
presented. 


Frame  Definitions  First,  some  comments  and  definitions  that  will  make  the  frame 
descriptions  clear  and  succinct: 

All  definitions  proceed  from  the  assumption  that  the  robot  base  is  resting  on  its 
intended  supports,  and  has  been  properly  leveled.  Left  and  right  are  used  as  descriptive 
terms,  and  assume  (unless  specified  otherwise)  that  the  viewer  is  head  up,  and  has 
his  back  to  the  front  mounting  plate  of  the  robot.  Similarly,  forward  is  defined  as 
the  direction  normal  to  the  front  mounting  plate,  and  directed  from  the  plate  into 
the  interior  of  the  workspace.  The  wrist  point  is  defined  as  that  point  (in  the  wrist 
mechanism)  where  orientation  is  independent  of  positioning  that  point.  All  coordinate 
frames  are  orthogonal  and  right  handed. 


Robot  frame  0  Robot  base  frame 
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Figure  2.2:  Schematic  showing  Frames  0  through  4 


Frame  4  Frame  5 


Frame  6 


Figure  2.3:  Schematic  showing  Frames  4  through  6  (dotted  indicates  previous  frame) 
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Frame  origin  The  intersection  of  the  shoulder  axis  and  the  horizontal  plane  that  con¬ 
tains  the  forearm  (distil  link)  centerline. 

X-f  Forward 

Y+  To  the  left 

Z-f  Vertical,  upward 

Robot  frame  1  Robot  upper  arm  frame 

Frame  origin  The  intersection  of  the  shoulder  axis  and  the  horizontal  plane  that  con¬ 
tains  the  forearm  (distil  link)  centerline. 

X+  la  horizontal  plane,  pointing  from  the  frame  origin  to  the  elbow  joint  axis. 

Y-f  In  horizontal  plane,  pointing  to  the  left  from  the  frame’s  Xf. 

Z-f  Vertical,  upward. 


Robot  frame  2  Robot  forearm  frame 

Frame  origin  The  intersection  of  the  elbow  axis  and  the  horizontal  plane  that  contains 
the  forearm  (distil  link)  centerline. 

X-f  In  horizontal  plane,  along  the  forearm  centerline,  pointing  from  the  origin  toward 
the  end  of  the  manipulator. 

Y-f  In  horizontal  plane,  pointing  to  the  left  from  the  frame’s  X+. 

Z-f  Vertical,  upward. 


Robot  frame  3  Robot  vertical  stage  frame 

Frame  origin  The  wrist  point  of  the  manipulator 

X-f  Always  parallel  to  X+  of  frame  2. 

Y+  Always  parallel  to  Y+  of  frame  2. 

Z-f  Always  parallel  to  Z+  of  frame  2.  (vertical,  upward.) 

Note  that  this  the  vertical  stage  of  the  robot  is  a  pure  translation,  thus  the  coordinate 
axes  remain  parallel  to  those  of  the  previous  frame. 
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Robot  frame  4  Robot  wrist  barrel  frame 
Frame  origin  The  wrist  point  of  the  manipulator 

X+  As  marked  on  the  wrist  barrel:  (X4+  is  always  in  the  horizontal  plane,  and  always 
perpendicular  to  Z5.  It  aligns  with  X3+  when  qj\  =  0.) 

Y+  As  marked  on  the  wrist  barrel:  (Y4+  is  always  in  the  horizontal  plane  and  parallel 
to  Z5;  it  aligns  with  Y3+  when  qj 4  =  0.) 

Z-f  Vertical,  upward. 


Robot  frame  5  Robot  wrist  yoke  frame 
Frame  origin  The  wrist  point  of  the  manipulator 

X+  As  marked  on  the  wrist  yoke:  always  in  the  X4-Z4  plane,  it  aligns  with  X44-  when 
qjs  =  0 

Y+  As  marked  on  the  wrist  yoke;  always  in  the  X4-Z4  plane,  it  aligns  with  Z4+  when 
qjs  =  0 

Z+  As  marked  on  the  wrist  yoke;  always  opposite  of  Y4+ 


Robot  frame  6  Robot  wrist  output  frame 
Frame  origin  The  wrist  point  of  the  manipulator 

X+  As  marked  on  the  wrist  output:  always  in  the  X5-Z5  plane,  it  aligns  with  X5+ 
when  qj$  =  0 

Y+  As  marked  on  the  wrist  yoke:  always  in  the  X5-Z5  plane,  opposite  Z5+  when 

qje  =  0 

Z-f-  As  marked  on  the  wrist  yoke:  always  parallel  to  Y5+ 

2.2.1  Forward  Kinematics  from  Motor  to  Joint  Space 

Now  that  we  have  defined  the  link  frames,  and  through  them  the  joint  coordinates  of 
these  manipulators,  we  can  present  the  first  part  of  the  forward  kinematics,  the  mapping 
that  takes  us  from  a  known  set  of  motor  coordinates  and  gives  us  the  joint  coordinates. 
Note  that  we  have  appended  a  coordinate  for  the  gripper  at  the  end  of  these  vectors,  so 
that  the  configuration  is  described  by  seven  coordinates. 
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~  0  0  0  0  0  0  '  qm\ 

—  —  0  0  0  0  ami 

0  0  0  0  0  0  qmz 

o  0  0  Jy  °  0  0  qmA  (2.1) 

0  0  0  0  ^5  ^6  0  9m5 

0  0  0  ±  £  2.  o  qm6 

oooooo  J  L  q™7 , 

Where  the  geometric  scale  factors  (usually  gear  ratios,  except  for  the  linear  joints) 


are: 

ns 

=  9.0 

ne 

=  3.0 

nz 

=  4947.380111  radians/meter 

n4 

=  250 

n5 

=  360 

n6 

=  360 

ngrip 

=  2782.901312  radians/meter 

2.2.1. 1 

Link  Transforms 

Having  defined  a  series  of  frames,  one  for  the  robot  base,  and  one  for  each  degree  of 
kinematic  freedom,  we  can  now  derive  the  homogeneous  transform  from  each  one  to  the 
next,  given  the  general  form  of  the  link  transform,  and  the  relevant  kinematic  data  in 
the  form  of  Denavit-Hartenberg  notation,  as  defined  in  [2].  Craig  defines  a  standard 
form  of  this  transform  in  his  equation  [3.6,  his  numbering.] 

He  also  assembles  the  kinematic  parameters  into  a  table,  such  that  the  quantities 
needed  to  write  the  transform  from  frame  i  to  the  next  frame  all  appear  on  line  i.  We 
can  make  the  process  straightforward  and  easy  to  automate  by  making  this  table  into 
a  matrix  with  the  number  of  rows  equal  to  the  number  of  degrees  of  freedom,  and  four 
columns  for  the  Denavit-Hartenberg  parameters  (in  the  order  a,_i,a<_i, d,, gj,  .)  We 
denote  this  matrix  as  KP,  for  kinematic  parameters.  We  can  then  write  the  general 
form  of  the  link  transform  as  follows: 

cosiKPi,*)  -sin  (KPij)  0  KPi,  2 

cos(K  Pi,i)  sin(K  P^)  cos(K  Pi,i)  cos(Ar  Pi, 4)  -s\n(KPi,i)  -KPi,zsin(KPi,i) 
ain(K  Pi,i)  sin(K  Pij)  sin(  K  Pi, 1)  cos(  K  Pi, 4)  cos(KPi,\ )  KPi,3  cos(A'P.,i) 

0  0  0  1 

(2.2) 

We  can  then  substitute  in  the  appropriate  entries  from  KP  to  get  the  six  transforms 
we  are  interested  in  having.  It  is  desirable  to  use  MACSYMA  to  do  this,  both  to 
eliminate  the  possibility  of  algebraic  error,  and  to  have  symbolic  help  in  simplifying  the 
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overall  transform,  which  is  a  product  of  six  such  matricies  and  very  complicated  looking 
before  simplification. 

Link  Parameters  and  Link  Transforms  The  KP  matrix  is  given  as  follows.  The 
joint  coordinates  are  qj i,qj2,dz,qj4, qjs ,  and  qj$.  .  .ngles  are  given  in  radians.  We 
have  left  /i  and  /2  as  symbols  for  generality,  ev^  '  iough  they  are  constants  and  are 
equal  (16  inches  =  40.64  cm  .)  Note  '*■  ~  .  number  of  zero  terms;  these  lead  to  a 

simple  kinematic  solution  for  the  manipulators,  even  though  they  have  all  the  geometric 
freedom  that  we  require. 


KP  = 


0 
0 
0 
0 

7r/2 

L  -*72 


0 

/i 

h 

0 

0 

0 


0 

0 

dz 

0 

0 

0 


m 

0 

qj 4 
qjs 

qje  j 


(2.3) 


Substituting  as  mentioned  above,  we  can  obtain  the  first  link  transform,  from  frame 
0  to  frame  1: 


C\  ~sx  0  0  ' 

si  ci  0  0 

0  0  10 

0  0  0  1 


(2.4) 


Where  c<  stands  for  cos (qji),Sij  stands  for  sin(qji  +  qjj),  etc.  —  a  convention  that 
we  adopt. 

In  the  same  manner,  we  write  the  transform  from  frame  1  to  frame  2: 


*T  = 


C2  -S2 
S2  C2 
0  0 
0  0 


The  transform  from  frame  2  to  frame  3: 


1  0  0 
0  1  0 
0  0  1 
0  0  0 

The  transform  from  frame  3  to  frame  4  is: 


c  4  -54 

S4  C4 
0  0 
0  0 


0  h  ' 
0  0 
1  0 
0  1 


0  0  ' 
0  0 
1  0 
0  1 


(2.5) 


(2.6) 


(2.7) 
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The  transform  from  frame  4  to  frame  5: 


c  5 

-55 

0 

0 

0 

0 

-1 

0 

55 

C5 

0 

0 

0 

0 

0 

1 

The  transform  from  frame  5  to  frame  6: 


Cg  —56  0  0 

0  0  10 
5g  0  0 

0  0  0  1 


(2.8) 


(2.9) 


2.2.2  Forward  Kinematics 

Now  that  we  have  the  six  individual  transforms,  we  can  begin  to  multiply  them  together 
to  obtain  the  forward  kinematics  of  increasingly  complete  fractions  of  the  total  manip¬ 
ulator.  We  present  the  results  in  this  fashion  in  order  to  show  that  the  geometry  we 
have  chosen  results  in  a  much  simpler  form  than  that  of  many  other  robots. 

The  keys  to  this  are  to  remember  that  our  geometry  maintains  the  first  four  joint 
axes  parallel,  and  that  the  upper  3  by  3  submatrix  of  the  overall  transform  specifies  the 
relative  orientation  between  the  frames  (as  direction  cosines.)  Therefore,  the  rotations 
about  axes  1,  2,  and  4,  are  all  about  a  parallel  axis  (joint  3  is  a  linear  translation  and 
doesn’t  effect  orientation.)  We  expect  to  be  able  to  simplify  the  rotation  parts  of  the 
forward  kinematics  up  to  frame  4  to  a  single  rotation  about  vertical.  This  is  the  case, 
and  the  kinematics  are  much  simpler  because  of  it. 

This  is  the  transform  from  frame  0  to  frame  2,  after  simplifying  using  the  sum  of 
angles  identities: 


C12  -512  0  llCi 

5l2  «12  0 

0  0  10 

0  0  0  1 


(2.10) 


We  can  now  multiply  out  the  transform  from  frame  0  to  frame  4  and  again  simplify 
using  the  sum  of  angles  identities: 

C124  -5i24  0  /2C12  + 

5124  0  I2S12  +  I1S1  /Olll 

0  0  1  dz  [  } 

.0  0  0  1 


This  is  the  complete  forward  kinematic  transform  from  frame  0  to  frame  6: 
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C5C6C124  -  363124 

-C63124  ~  C5S6C124 

-35C124 

2r  = 

C5C6S124  +  36-3124 

C«Cl24  -  C5363124 

-•*5-3124 

S5C6 

-35C6 

C5 

0 

0 

0 

It  should  be  noted  that  the  simplification  to 

this  form 

hcu  +  hc\ 
hau  +  hai 

^3 

1 


(2.12) 


edge  about  the  geometry.  Attempts  to  use  MACSYMA  in  a  brute  force  manner, 
without  taking  advantage  of  the  structure  of  the  system  —  the  command  TRIGRE- 
DUCE(Tl.T2.T3.T4.T5.T6)  —  results  in  an  expression  that  is  more  complicated  than 
this  result  (in  terms  of  number  of  trig  evaluations)  and  possibly  more  complicated  than 
the  original.  Thus,  symbolic  methods  are  not  a  substitute  for  insight  into  the  problem, 
they  are  a  tool  whose  use  is  still  something  of  an  art. 


Comparison  to  the  PUMA  560  A  brief  comparison  to  the  forward  kinematics  of 
a  PUMA  560  is  instructive.  Craig  derives  these  in  his  book  as  eq.  3.14.;  we  do  not 
need  to  reproduce  them  here.  Even  in  factored  form,  the  most  complex  element  of  the 
PUMA’s  forward  transform  requires  the  evaluation  of  11  different  trigonometric  terms; 
the  most  complex  for  our  arms  requite  only  five.  This  overstates  the  case  ,  as  many 
of  the  trig  terms  are  reused  in  different  numbers  in  the  two  different  kinematics  we 
are  comparing,  and  the  reuse  of  trig  terms  makes  the  advantage  (measured  solely  in 
terms  of  trigonometry)  considerably  less.  However  the  complexity  in  terms  of  other 
arithmetic  operations  still  favors  our  arm  substantially.  The  same  PUMA  term  has 
15  other  arithmetic  operations;  ours  has  four.  Again,  there  is  more  reuse  of  common 
subexpressions  in  the  PUMA  solution,  which  can  reduce  computation  time  if  these 
can  be  stored  and  later  recalled  faster  than  recomputing  them.  Our  purpose  is  not  to 
criticize  the  designers  of  the  PUMA  (some  of  whom  have  given  us  valuable  guidance 
on  this  and  other  research.)  But,  overall,  we  have  succeeded  in  developing  a  pair  of 
manipulators  whose  geometry  is  much  simpler  to  deal  with  in  realtime  control,  enabling 
us  to  concentrate  our  efforts  on  the  dynamics  (which  are  also  simplified  for  the  same 
reasons)  and  control  aspects  of  cooperation  and  flexibility. 


Solving  for  the  Cartesian  Vector  Although  this  is  a  correct  formal  representation, 
we  are  more  interested  in  solving  for  the  cartesian  representation  of  the  configuration 
vector,  given  the  joint  vector,  and  vice  versa.  Thus,  we  do  not  usually  need  to  represent 
orientation  by  the  3  by  3  matrix  of  direction  cosines,  except  perhaps  as  an  intermediate 
step  in  calculations.  We  can  pull  out  of  the  transform  the  terms  that  enable  us  to  solve 
for  the  cartesian  description  of  the  arm’s  configuration  given  the  joint  space  description. 
Taking  the  terms  in  the  last  column  of  transform  matrix  in  2.12,  we  have: 


qcx  =  lC\2  +  lc\  =  1(C\2  +  Cl) 


(2.13) 
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qcy  =  lsi-2  +  hi  =  l{sn  +  3i)  (2.14) 

where  we  have  made  the  previously  noted  simplification  l  =  l\  =  h 

qcz  =  qjx  =  qh  (2-15) 

Given  our  stated  definition  of  orientation  in  the  cartesian  vector  as  a  series  of  rota¬ 
tions  that  begin  aligned  with  the  rererence  frame,  then  we  can  determine  the  three  roll, 
pitch,  and  yaw  angles  in  cartesian  coordinates  directly,  without  going  through  interme¬ 
diate  direction  cosine  matricies.  Yaw  in  cartesian  coordinates  is  the  sum  of  the  revolute 
joint  angles  between  the  base  frame  and  the  main  wrist  barrel.  All  of  these  rotations  are 
about  a  vertical  axis,  and  all  have  the  same  sense  of  sign.  So  cartesian  yaw  is  the  sum 
of  the  shoulder  joint  angle,  the  elbow  joint  angle,  and  the  wrist  yaw  angle  (the  vertical 
translation  of  joint  three  does  not  effect  yaw.) 

q<hi  =  qjt  +  qjt  +  qjy  -  qji  +  qji  +  qj*  (2-16) 

Given  our  choice  of  orientation  description  in  the  two  coordinate  systems,  pitch  and 
roll  (and  gripper  opening,  as  well)  translate  across  identically: 


qcp  =  qjP 

(2.17) 

qcr  =  qjt 

(2.18) 

qcg  =  qjg 

(2.19) 

2.2.3  Inverse  Kinematics 

We  now  consider  the  problem  of  inverse  kinematics.  The  first  aspect  of  the  inverse  kine¬ 
matics  presented  here  is  the  solution  for  the  robot’s  configuration  in  joint  space,  given 
its  configuration  in  the  cartesian  base  frame  (frame  zero  as  defined  previously.  There  is 
one  additional  piece  of  information  that  we  need  in  order  to  make  this  relationship  un¬ 
ambiguous:  we  need  to  specify  whether  we  want  a  right-handed  or  left-handed  solution. 
Left-handed  coresponds  to  qji  <  0,  right  handed  coresponds  to  qji  >  0.  The  inverse 
kinematic  solution  is  best  done,  term  by  term  in  sequence.  We  begin  by  applying  the 
law  of  cosines  to  the  triangle  formed  by  the  first  two  links  (the  third  side  is  the  line  from 
shoulder  joint  to  the  end  of  the  second  link.) 

ci  =  cos  (qji)  =  -■■12p?C2  -  1  (2.20) 

The  law  of  cosines  calculation  is  simplified  because  we  have  deliberately  made 
li  =s  li  =  /(for  notation)-  in  implementation,  2 12  is  a  precomputed  constant.  We  then 
calculate  qji,  using  the  arccosine,  and  calculate  the  sine,  for  use  later. 
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qj2  =  i  cos  l(c2)  (2.21) 

where  positive  means  a  right  handed  solution,  negative  is  left  handed. 

s2  =  sin  (qj2)  =  (2.22) 

We  must  then  handle  some  special  cases  to  get  the  shoulder  angle  right  under  all 
reachable  conditions,  when  qcx  =  0,  we  would  be  dividing  by  0  when  we  attempt  to 
calculate  the  tangent  of  the  shoulder  angle,  thus  we  have  two  special  cases: 

qjx  —  ic  —  for  qcx  =  0,  and  qcy  >  0  (2.23) 

qjx  =  -r  -  for  qcx  =  0,  and  qcy  <  0  (2  24) 

2 

qjx  —  tan-1(~-)  -  otherwise  (2.25) 

Since  vertical  position  of  the  wrist  point  is  the  same  in  cartesian  and  joint  spaces, 
the  third  term  of  the  inverse  kinematics  is  available  by  inspection, 

qj*  =  qh  =  ?c3.  (2-26) 

The  fourth  term,  (yaw)  is  simple,  because  ail  of  the  rotations  so  far  have  been  about 
the  same  vertical  axis.  So,  the  yaw  coordinate  is: 


qj <  =  qjy  =  qcy  -  ( qj ,  +  qje)-  (2-27) 

Given  our  choice  of  representation  for  orientation  in  the  cartesian  configuration,  we 
have  simple  term  by  term  equalities  between  the  joint  and  cartesian  vectors  for  the  fifth 
through  seventh  terms. 


qjs  =  qjp  -  q<h>  (2-28) 

qje  =  qjr  =  qcr  (2.29) 

qjr  =  qjg  =  qcg  (2-3°) 


26 


CHAPTER  2.  COOPERATING  FLEXIBLE-DRIVE  MANIPULATORS 


2.2.4  The  Jacobian  Matrix 

The  Jacobian  matrix  can  now  be  calculated  from  the  kinematic  relations  we  have.  The 
Jacobian  is  required  for  many  control  algorithms.  It  can  be  thought  of  as  an  instanta¬ 
neous  linear  mapping  from  joint  velocities  to  cartesian  velocities  (or  equivalently  from 
joint  forces/torques  to  cartesian  forces/torques.)  Our  approach  is  to  solve  for  the  carte¬ 
sian  velocities  on  a  term  by  term  basis,  and  then  assemble  these  six  equations  into  matrix 
form  with  the  variables  in  the  desired  order.  The  first  three  cartesian  quantities,  the  lin¬ 
ear  velocities  in  the  x,  y,  and  z  directions  in  the  base  frame,  can  be  found  by  taking  the 
derivative  of  the  last  column  of  §T,  which  expressed  the  forward  kinematic  transform. 
The  velocities  (or  rates)  are  referred  to  as  u’s,  with  the  rest  of  the  nomenclature  for  the 
three  principal  coordinate  systems  similar  to  that  for  the  q’s  except  that  the  cartesian 
orientation  rates  are  defined  and  named  slightly  differently:  The  motor  space  velocity 
vector  is  defined  to  be  the  time  derivative  of  the  motor  space  configuration  vector,  and 
the  joint  space  velocity  vector  is  defined  analogously: 


um  =  qin 

(2.31) 

. 

uj  =  qj 

(2.32) 

The  cartesian  velocity  vector  is  not  simply  the  time  derivative  of  the  (base  frame) 
cartesian  configuration  vector.  Following  the  convention  of  [2],  we  define  the  (base 
frame)  cartesian  velocity  vector  as  follows. 

ucx 

ucy 

°v  t icz 

uc  =  °u  =  ucu,x  (2.33) 

qcg  tiCvY 

uc^z 

U  Cg 

where  °o  is  the  cartesian  velocity  (in  the  base  frame)  of  frame  six’s  origin  and  °u> 
is  the  angular  velocity  of  frame  six,  expressed  in  the  base  frame  and  ucg  is  the  gripper 
separation  velocity.  This  follows  [2]  equation  5.36.  The  first  three  components  in  uc 
are  the  time  derivatives  of  the  coresponding  terms  in  qc,  so  the  first  three  components 
can  be  found  by  straightforward  differentiation  of  the  term  by  term  forward  kinematics 
for  position,  2.13  through  2.15.  Performing  these  differentiations,  we  get  the  following 
three  equations: 

ucx  =  -L(siujl-Mn(ujl  +  u;2)) 

ucy  =  +L(c\ujl  +  ci2(u;l  +  uj'2)) 

ucx  =  uj3 


(2.34) 
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In 


with: 


matrix  form,  the  first  three  DOF  are  independent  of  the  latter  three,  leaving  us 


'  ucx 

-L(si+S2)  -L(sn)  0 

‘  uji  ' 

ucy 

= 

i(ci  +  ci2)  -L(ci2)  0 

uj2 

uez 

0  0  ! 

.  u7  3  . 

(2.35) 


Obtaining  the  equations  for  the  last  three  cartesian  u’s,  namely  u cwx,  ucwy,  and 
«Cu»z,  are  more  complex.  These  quantities  are  the  last  link’s  angular  velocity,  resolved 
into  components  about  the  unit  vectors  of  the  base  frame.  Note  that  these  are  not 
simply  the  derivatives  of  the  last  three  coordinates.  To  get  the  desired  expressions,  we 
must  first  compute  the  angular  velocity  of  the  last  link  (and  this  calculation  is  best 
done  in  that  body’s  attached  frame),  and  then  transform  that  back  into  the  base  frame. 
The  angular  velocity  is  not  merely  the  derivatives  of  the  joint  rates,  instead,  it  can  be 
computed  by  propagating  the  angular  velocity  from  link  to  link  by  applying  Craig  5.16 
once  for  each  link.  The  result  is  the  angular  velocity  of  the  last  (sixth)  link,  resolved 
into  components  of  its  attached  frame: 


V- 


«SC6«Jl24  “  S6ujS 
-C«UJ5  -  S5Seuju4 

+  C5UJ124 


(2.36) 


Now  we  want  to  express  this  quantity  in  the  base  frame,  so  we  multiply  by  gft,  which  is 
the  upper  3  by  3  matrix  in  %T.  After  expanding  all  of  the  terms  and  simplifying  using 
trig  identities,  we  get 


0  6 
V  = 


$124 -  S5Ci24U?6 
-SSSi24Uj6  -  Ci24«j'5 

c5u;6  +  u;'i24 


(2.37) 


where  ujlu  =  uji  +  uj2  +  u jA. 

We  can  now  write  the  equation  for  cartesian  velocity  as  a  function  of  joint  velocity 
in  matrix  form  by  combining  equations  2.35  and  2.37. 


uc 


=JJuj 


(2.38) 


where 


-L(s\  +  S12) 
+L(ci  +C12) 
0 
0 
0 
1 
0 


-//(S12)  0 
-L(cjj)  0 
0 


0 

0 

1 

0 


0 

0 

0 

0 

0 

1 

0 


0 

0 

0 

$124 

-C124 

0 

0 


0 

0 

0 

-C124S5 

-5124^5 

Cs 

0 


0 

0 

0 

0 

0 

0 

1 


(2.39) 
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The  Jacobian  has  some  important  structural  features  that  have  bearing  on  the  com¬ 
plexity  and  robustness  of  control  algorithms.  The  matrix  is  almost  block  diagonal,  and 
the  blocks  are  small  (one  3x3,  one  2x2,  and  two  lxl  blocks).  The  only  terms  that 
are  not  block  diagonal  are  the  first  two  ones  in  the  next  to  the  bottom  row.  These  terms 
express  the  fact  that  the  shoulder  and  elbow  joint  rates  add  directly  to  the  last  link’s 
angular  velocity  about  the  base  frame  Z  axis.  This  is  a  simple  one  way  coupling.  The 
important  results  of  this  structure  are  twofold.  The  Jacobian,  being  fairly  sparse  and 
simple,  can  be  computed  rapidly.  Secondly,  we  can  easily  see  when  the  Jacobian  will  go 
singular  (lose  rank).  First  we  consider  the  uppermost  2x2  block,  namely 


— L{S\  +  3ij)  -T(Si2) 
+L(c\  +  C12)  -I-(ci2) 


,  if  we  let  qji  =  0  or  qji  =  ±ir,  then  this  submatrix  becomes  singular.  Physically, 
we  can  understand  these  cases  as  follows:  When  qj?  =  0,  the  arm  is  straight  out,  with 
the  wrist  point  as  far  out  from  the  base  frame  origin  as  it  can  go;  in  this  configuration, 
incremental  motions  of  either  the  shoulder  joint  or  the  elbow  joint  will  move  the  wrist 
point  from  side  to  side,  but  will  cause  no  motion  normal  to  the  workspace  boundary  (in 
or  out).  Similarly,  when  qji  =  ±ir,  the  manipulator  is  folded  back  on  itself,  and  incre¬ 
mental  motions  of  the  shoulder  or  elbow  cannot  cause  motion  normal  to  the  workspace 
boundary.  In  practice,  the  elbow  is  prevented  from  reaching  ±x,  by  mechanical  limit 
stops,  in  order  to  protect  the  wrist  mechanism,  so  this  case  does  not  occur  physically. 
These  cases  are  called  workspace  boundary  singularities.  Next  we  consider  the  3x3 
block,  namely 

0  «124  -Cl24*5 

0  —  C124  -$124^8 
10  cs 

,  when  qjs  =  0  or  qjs  =  ±t,  then  this  submatrix  becomes  singular.  Again,  the 
±ir  is  outside  the  limit  stops  on  the  mechanism,  and  is  not  a  problem.  However,  the 
case  of  qjs  =  0  is  precisely  the  reference  configuration  for  the  fifth  joint.  When  qjs  = 
0,  the  fourth  and  sixth  joint  axes  are  aligned  with  each  other  (both  vertical);  this 
condition  is  called  a  workspace  interior  singularity  (also  known  as  gimbal  lock,  a  term 
from  gyroscopes,  which  also  have  the  same  mechanism  geometry  as  our  wrist).  In  this 
configuration,  the  robot  loses  one  degree  of  orientation  freedom  (cannot  rotate  about 
the  4X  axis),  and  gains  redundancy  in  another  (joints  4  and  6  are  both  vertical).  Special 
exception  handling  software  must  take  this  limitation  of  the  mechanism  into  account, 
and  the  human  or  AI  system  that  performs  the  task  planning  must  be  aware  of,  and 
properly  plan  for,  this  situation.  This  problem  could  be  sidestepped  by  limiting  the 
range  of  motion  of  the  fifth  axis  to  0  <  qjs  <  *  or  0  >  qjs  >  — w,  but  this  would  be 
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artificially  discarding  half  of  the  mechanism’s  reachable  orientations,  a  gross  limitation 
of  the  dexterous  workspace.  This  problem  is  fundamental  to  this  class  of  wrists,  and 
has  been  a  problem  for  many  6  degree-of- freedom  manipulators. 


2.3  Identification  and  Control  Experiments 

In  this  section,  we  present  the  principal  results  from  the  identification  and  control  ex¬ 
periments  performed  using  the  cooperating  flexible-drive  manipulators.  The  kinematic 
parameters  (link  parameters  and  gear  ratios)  have  been  presented  in  the  previous  sec¬ 
tion.  In  the  sections  below,  we  present  the  results  of  experiments  designed  to  identify 
important  aspects  of  the  dynamic  model,  namely  mass  properties  and  friction-flexibility 
properties  of  the  manipulators. 


2.3.1  Mass  Property  Identification  Experiments 

Knowledge  of  the  mass  properties  is  essential  to  an  accurate  dynamic  model  of  the 
manipulator.  The  mass  properties,  together  with  the  kinematic  model,  are  the  heart 
of  the  ideal  mechanism  dynamics  -  the  dynamics  of  a  manipulator  with  absolutely  no 
friction  or  flexibility.  The  properties  that  we  need  are  the  mass,  location  of  the  center 
of  mass,  and  the  inertia  properties  (about  the  mass  center)  for  each  link.  Often,  due 
to  the  geometry  of  a  manipulator,  some  of  this  information  is  not  used  in  the  model, 
and  is  thus  not  needed.  For  example,  the  first  four  links  of  our  manipulator  rotate 
only  about  vertical  axes;  thus  they  never  experience  angular  accelerations  about  any 
other  axis,  and  the  only  relevant  inertia  properties  are  their  inertias  (with  respect  to 
their  respective  mass  centers)  about  their  vertical  axes.  A  table  of  the  most  important 
parameters  follows: 


||  Assembly  name 

Mass  Center  Location  (M.) 

Mass  (Kg.) 

Inertia  Term 

Inertia  (KgAf2) 

Upper  Arm 

X\  +0.150 

4.902 

/« 

0.141 

Forearm 

X2  +0.225 

6.345 

/„ 

0.222 

Grippet 

n/a 

0.6105 

0.001749 

The  actuator  scale  factors  (in  our  case,  these  are  all  motor  torque  constants)  together 
with  the  geometric  scale  factors,  and  the  scale  factors  associated  with  the  interface 
electronics,  define  the  corespondence  between  commands  from  the  computer  to  torques 
generated  at  the  motors  (exclusive  of  friction).  The  scale  factors  for  both  the  actuators 
and  the  associated  electronics  are  presented  here.  The  interface  electronics  also  impose 
current  (and  thus  torque)  limits  on  the  actuators;  for  the  shoulder  and  elbow  actuators, 
these  have  two  settings:  a  low  power  setting  for  safe  testing  of  controllers  and  a  full 
power  setting.  These  values  are  tabulated  below. 
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Actuator 

Torque  const. 
(N-m/Amp) 

Amplifier  const. 
(Amps/volt) 

Max.  Torque 
(N-m) 

Low  Power  Torque 
(N-m) 

|  Shoulder 

0.941 

1.00 

9.41 

1.88 

Elbow 

0.941 

1.00 

9.41 

1.88 

Z  Axis 

0.0224 

0.50 

0.112 

0.112 

Yaw 

0.0224 

0.20 

0.0447 

0.0447 

Roll  (motor  5) 

0.0224 

0.20 

0.0447 

0.0447 

Pitch  (motor  6) 

0.0224 

0.20 

0.0447 

0.0447 

1  Gripper 

0.0224 

0.20 

0.0447 

0.0447 

Actuator  scale  factors  are  the  nominal  values  provided  by  the  manufacturers;  the 
amplifier  scale  factors  were  measured  experimentally,  as  were  the  peak  torques  at  both 
maximum  and  the  low  power  settings. 

2.3.2  Flexibility  and  Friction  Identification  Experiments 

Identification  and  control  experiments  have  been  performed  on  the  manipulator,  with 
particular  emphasis  on  understanding  the  effects  of  joint  flexibility  on  the  system’s  per¬ 
formance.  One  of  the  foundations  of  our  approach  to  cooperative  control  of  flexible 
drive  manipulators  is  our  ability  to  model  and  control  the  torque  delivered  to  each  link 
through  the  flexible  drive  system.  The  subsystem  of  actuator,  flexible  drivetrain,  and 
link  is  modeled  as  a  free,  free,  mass-spring-mass  system  (while  not  in  contact  with  any¬ 
thing  in  the  environment  [6]).  Thus,  in  our  identification  and  initial  control  experiments, 
we  look  for  evidence  of  response  dominated  by  second  order,  lightly  damped  behavior. 
In  the  experimental  data  shown  in  figure  2.3.2,  we  see  clear  evidence  that  the  elbow 
subsystem’s  behavior  coresponds  to  our  model’s  prediction.  Additionally,  we  see  the 
evidence  of  some  nonlinearities  -  friction  (and/or  cogging)  -  having  a  substantial  effect 
on  the  amplitude  response.  Thus  we  see  clearly  the  need  for  more  research. 


2.4  Continuing  Research 

Research  on  cooperating,  flexible  drive  manipulators  will  continue  under  DARPA  con¬ 
tract  DAAA21-89-C-0002,  with  principal  investigators  Professor  Robert  H.  Cannon,  Jr. 
in  the  department  of  Aeronautics  and  Astronautics  and  Professor  Jean-Claude  LaTombe 
in  the  department  of  Computer  Science.  Under  this  new  contract,  research  into  the 
dynamics  and  control  of  cooperating,  flexible  manipulators  will  continue,  and  will  be 
integrated  with  research  on  motion  and  task  planning.  The  continuing  work  in  dynamics 
and  control(s)  will  emphasize  achieving  levels  of  dynamic  control  that  will  permit  the 
demonstration  of  good  solutions  to  previously  intractable  problems  for  automation. 

The  new  contract  emphasizes  research  in  three  main  areas:  (1)  the  specific  tech¬ 
nologies  that  are  critical  to  achieving  true,  effective  integrated  cooperation  between 


SIDF  Normalized  Amplitudes 
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Frequency  in  Hz. 


Figure  2.4:  Model  and  experimental  response  to  a  sinusoidal  input:  describing  function 
data  for  the  elbow  subsystem  excited  by  a  2.0  and  1.0  volts  peak-to-peak  input.  The 
solid  line  is  the  model,  the  dotted  line  the  data  at  2.0  V.,  and  the  dashed  line  the  data 
at  1.0  V. 
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compliant  manipulators;  (2)  motion  planning  and  execution,  for  both  gross  and  fine 
maneuvering  of  awkward  objects  amidst  obstacles,  so  that  users  can  specify  object  mo¬ 
tions  per  se  at  a  high  level;  and  (3)  integration  of  the  path  planning  and  manipulation 
functions.  Specifically,  we  will  demonstrate  experimentally  the  intimate  vertical  in¬ 
tegration  of  path  planning  with  the  manipulative  skills  of  a  pair  of  deft,  cooperating 
robotic  arms  in  the  rapid  assembly  of  a  complex  configuration  of  awkward  objects,  all 
in  response  to  only  high-level  instructions. 


Chapter  3 

Control  of  a  Flexible  Robot  with 
a  Mini-Manipulator 


Raymond  Kraft 


3.1  Introduction 

Present  day  industrial  robots  are  slow  and  massive.  Their  motions  are  controlled  by 
inferring  end-point  position  from  joint  angle  measurements,  and  end-point  force  from 
joint  torque  measurements.  This  is  known  as  collocated  control;  that  is,  sensing  and 
actuation  take  place  at  the  same  physical  location.  Naturally,  if  there  is  any  flexibility 
in  the  system  —  something  present  in  all  real  physical  systems  —  the  actual  end-point 
position  and  force  will  not  agree  with  what  is  predicted  by  the  joint  angles  and  torques. 
Consequently,  robots  have  been  designed  to  be  very  rigid,  and  hence  very  heavy,  so  as 
to  minimize  errors  introduced  by  flexibility. 

The  obvious  solution  to  this  problem  is  to  directly  measure  those  quantities  that  axe 
to  be  controlled.  Namely,  to  measure  the  end-point  position  and  force,  and  to  feed  these 
measurements  back  to  a  controller  that  drives  the  actuators.  Given  such  a  controller, 
this  scheme  would  eliminate  flexibility  induced  errors,  and  would  hence  enable  one  to 
use  lighter-weight,  flexible,  robots.  There  is,  however,  a  good  reason  why  this  approach 
has  not  been  used  in  the  past:  it  poses  a  much  more  difficult  stability  problem.  Where 
it  is  easy  to  achieve  stable,  albeit  slow,  control  through  the  use  of  collocated  control,  it 
is  non-trivial  to  achieve  good,  stable  control  using  end-point  feedback. 

While  end-point  feedback  increases  the  speed  and  precision  of  manipulation,  there  is 
still  a  fundamental  problem  inherent  in  controlling  robots  that  are  designed  to  operate 
over  a  large  workspace.  Such  a  robot  must  obviously  be  large  in  size,  and  consequently 
it  becomes  difficult  to  achieve  rapid,  precise  end-point  control  using  an  actuator  that 
is  far  removed  from  the  end-point.  This  difficulty  may  be  attributed  to  two  factors: 
(1)  fine  end-point  motions  of  a  large  manipulator  translate  into  yet  finer  joint  angle 
motions,  and  (2)  the  flexibility  in  the  manipulator  limits  end-point  control  bandwidth 
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to  the  time  it  takes  a  bending  wave  to  travel  the  length  of  the  manipulator. 

A  means  of  sidestepping  this  problem  is  to  equip  the  manipulator  with  a  small, 
lightweight,  rigid  mini-manipulator  located  at  the  end-point  of  the  main  manipulator. 
The  mini-manipulator  is  then  used  for  rapid,  precise  control  within  a  small  workspace, 
while  the  main  manipulator  is  used  to  ferry  the  mini-manipulator  between  widely  sep¬ 
arated  workstations. 

Thus,  by  using  end-point  control  in  conjunction  with  a  mini- manipulator,  a  lightweight 
robot  would  be  able  to  achieve  high  bandwidth,  precise  control  over  a  large  workspace. 
It  is  these  considerations  which  motivated  the  research  on  position  and  force  control  of 
a  flexible  robot  arm  with  a  two-degree-of- freedom  mini-manipulator. 


3.2  Experimental  Apparatus 

Figure  3.1  depicts  the  experimental  system.  It  is  comprised  of  the  following  major  sub¬ 
systems:  the  flexible  arm,  the  mini-manipulator,  actuators,  sensors,  and  a  target.  The 
entire  manipulator  system  operates  in  the  horizontal  plane  with  the  end-point  of  the 
flexible  arm  following  a  circular  arc. 

3.2.1  The  Flexible  Arm 

The  flexible  arm  has  distributed  flexibility  and  was  intentionally  designed  to  exaggerate 
structural  flexibility  in  the  horizontal  plane  while  remaining  stiff  in  the  gravity  influenced 
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vertical  direction.  The  purpose  of  exaggerating  the  horizontal  flexibility  in  this  system 
was  two  fold:  (1)  to  make  problems  associated  with  flexibility  more  readily  apparent,  and 
(2)  to  lower  the  resonant  frequencies  of  the  flexible  arm  into  a  region  where  reasonable 
sampling  rates,  in  the  neighborhood  of  25  to  100  Hz,  could  be  used.  The  flexible  arm 
consists  of  a  hub  inertia  block,  two  thin  aluminum  plates  as  side-rails,  and  fourteen 
bridges  which  separate  the  side-rails  and  are  connected  to  them  by  beryllium  copper 
springs.  The  flexible  arm  and  the  hub  inertia  block  are  1.04  meters  long,  weigh  3.1  kg, 
and  have  an  inertia  about  the  hub  of  0.45  kg-m2.  The  two  side-rails  are  1  mm  thick,  6.6 
cm  wide,  and  are  made  of  2024-T3  grade  aluminum.  The  main  actuator  for  the  flexible 
beam  is  the  hub  motor  located  at  its  base  and  below  the  table.  A  potentiometer  is 
collocated  with  the  hub  motor  and  is  used  to  measure  the  base  angle  of  the  flexible  arm. 
A  light  emitting  diode  (LED)  array,  whose  position  can  be  sensed  by  a  photodetector, 
is  located  at  the  end-point  of  the  flexible  arm. 

3.2.2  The  Mini-Manipulator 

The  mini-manipulator  is  attached  to  the  end-point  of  the  flexible  arm  along  one  of  the 
side-rails.  Essentially,  it  is  a  five  link,  closed  kinematic  chain  —  the  base  link  being  fixed 
to  the  flexible  arm  (see  Fig  3.10  for  a  close-up  view  of  the  mini-manipulator).  The  base 
link  measures  7.6  cm  in  length.  Each  of  the  two  inboard  links  are  7.0  cm  long  and  are 
rigidly  attached  to  the  two  motors  that  drive  the  mini-manipulator.  The  two  outboard 
links  are  10.8  cm  long.  Located  at  the  joint  between  the  two  outboard  links  is  another 
LED  array  which  is  used  to  sense  the  position  of  the  end-point  of  the  mini-manipulator. 

Below  this  LED  array  is  a  vertical,  force  sensing,  aluminum  beam.  It  is  approxi¬ 
mately  9.5  cm  long  and  is  equipped  with  strain  gauges  which  provide  a  means  of  mea¬ 
suring  end-point  force.  At  the  bottom  end  of  the  force  beam  is  a  circular  contact  roller, 
which  allows  the  mini-manipulator  to  exert  only  normal  forces  on  objects  it  comes  into 
contact  with.  The  entire  five  link  chain,  along  with  the  force  beam  and  contact  roller, 
weighs  approximately  0.14  kg. 

3.2.3  Actuators 

The  hub  motor  is  an  Aeroflex  TQ-52W  limited  angle,  brushless  DC  torquer  with  a  peak 
torque  of  1.7  N-m  and  a  linear  range  of  ±45  degrees.  The  mini- manipulator  motors  are 
small,  rare  earth  magnet,  DC  torquers  with  0.15  N-m  of  peak  torque. 

Each  of  the  motors  is  driven  by  a  linear  power  amplifier  that  produces  an  output 
current  proportional  to  an  input  voltage.  These  amplifiers  have  a  bandwidth  of  about 
1kHz,  which  allows  them  to  be  modeled  as  a  pure  gain  for  the  flexible  arm  and  mini¬ 
manipulator  system. 

3.2.4  Sensors 

The  hub  angle  is  measured  by  a  conductive  plastic  potentiometer  mounted  on  the  shaft 
of  the  hub  motor.  The  hub  potentiometer  has  a  sensitivity  of  about  3.4  Volts  per  radian. 
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Angular  rate  is  obtained  by  passing  the  signal  of  the  potentiometer  through  an  analog 
pseudo-differentiator  whose  transfer  function  is  . 

Position  sensing  is  achieved  through  the  use  of  an  overhead  photodetector  which 
measures  the  Cartesian  coordinates  of  a  single  light  source.  To  enable  the  tracking  of 
multiple  targets,  a  time  multiplexing  scheme  is  used,  where  multiple  LED  arrays  are 
sequentially  flashed  on  and  off  at  1kHz.  LED’s  mounted  at  the  end-point  of  the  flexible 
arm  and  at  the  end-point  of  the  mini-manipulator  provide  a  means  of  measuring  these 
two  end-point  coordinates.  A  finely  tuned  analog  notch  filter  is  used  to  filter  out  60 
Hz  noise  present  in  the  system.  Additionally,  these  position  measurements  are  passed 
through  analog,  single  pole,  low-pass  filters  to  remove  other  high  frequency  noise.  The 
cut-off  frequencies  for  measurements  of  the  end-point  of  the  flexible  arm  and  the  mini¬ 
manipulator  are  10  Hz  and  40  Hz  respectively.  Overall,  position  sensing  accuracy  is 
roughly  ±0.5  mm  at  a  gain  of  about  0.1  Volts  per  centimeter. 

The  angular  positions  of  the  two  inboard  mini-manipulator  links  are  measured  by  two 
conductive  plastic  potentiometers  mounted  on  the  shafts  of  the  mini-manipulator  mo¬ 
tors.  The  potentiometers  have  a  sensitivity  of  about  3.4  Volts  per  radian.  Angular  rates 
are  obtained  by  passing  the  potentiometer  signals  through  analog  pseudo-differentiators 
with  transfer  function  7^555. 

The  forces  applied  to  the  end-point  of  the  mini-manipulator  are  measured  by  an 
aluminum  force  beam.  It  is  9.5  cm  long  and  has  a  5mm  square  cross  section.  Semi¬ 
conductor  strain  gauges  are  mounted  on  all  four  sides  at  the  root  of  the  force  beam. 
Thus  Cartesian  components  of  force  in  a  local  reference  frame  can  be  measured  by 
connecting  opposing  pairs  of  strain  gauges  in  a  bridge  configuration,  and  amplifying  the 
bridge  output.  The  force  signals  are  filtered  by  a  third  order  Bessel  type  filter  with 
poles  at  a  =  —151.6  and  a  =  -334.0  ±  530-2j.  Overall,  the  force  sensor  has  a  gain  of 
roughly  14  grams  force  per  Volt,  with  a  noise  level  of  about  1.5  mV. 


3.2.5  The  Target 

A  target  was  used  to  demonstrate  force  control  and  surface  following.  It  was  cut  from 
plexiglas  on  a  Matsuura  CNC  machine.  It  is  11  inches  long,  2.5  inches  wide,  and  1.5 
inches  thick.  It  offers  a  variety  of  complex  contours  from  flat  surfaces,  to  curved  surfaces, 

to  sharp  corners. 


3.2.6  The  Real  Time  Computer 

Real  time  control  was  achieved  using  a  DEC  PDP  11/83  mini-computer  with  an  RT- 
11  operating  system.  The  system  has  a  floating  point  math  board,  16  twelve-bit  A/D 
converters,  and  12  twelve-bit  D/A  converters.  All  control  programs  were  written  in 
FORTRAN  77  and  PDP  assembly  language. 
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3.2.7  Identification  Equipment 

A  Schlumberger  Solartron  model  1254  frequency  response  analyzer  was  used  to  identify 
the  dynamics  of  the  experimental  system.  The  frequency  response  analyzer  generates  a 
sinusoidal  signal  which  is  used  to  drive  the  system’s  actuators.  The  resulting  motions 
and  forces  are  then  measured  by  the  analyzer,  and  their  magnitudes  and  phases  relative 
to  the  input  are  calculated. 


3.2.8  Other  Computing  Equipment 

Control  law  synthesis  and  simulation  was  done  on  both  VAX  and  SUN  workstations. 
The  commercially  available  control  packages  MATRIXx  and  PRO-MATLAB  were  used. 


3.3  Modifying  the  Plant  for  Favorable  Dynamics 

One  problem  that  was  encountered  in  doing  end-point  control  of  the  mini- manipulator 
was  the  presence  of  torsion  in  the  flexible  arm.  The  flexible  arm  had  been  designed  to 
ideally  be  flexible  only  in  the  horizontal  direction,  and  with  earlier  experiments  that 
used  similar  flexible  arms,  no  torsion  related  problems  were  encountered.  However,  the 
current  two-degree-of-freedom  mini-manipulator  mounted  at  the  end  of  the  flexible  arm 
resulted  in  the  flexible  arm  having  a  much  higher  end-point  mass  and  inertia  than  pre¬ 
vious  experimental  arms.  This  in  turn  resulted  in  a  much  lower  frequency  for  torsional 
oscillations.  The  frequency  of  the  first  torsion  mode  was  approximately  10  Hz  —  just 
within  the  desired  10  Hz  mini-manipulator  control  bandwidth. 

Further  complicating  matters,  it  was  discovered  that  the  transfer  function  from 
mini- manipulator  end-point  force  to  mini-manipulator  end-point  position  had  a  first 
resonance  pole  at  a  lower  frequency  than  the  first  resonance  zero  —  an  unfavorable 
pole-zero  configuration. 

3.3.1  Favorable  and  Unfavorable  Pole-Zero  Patterns 

Basically,  the  transfer  function  from  mini-manipulator  end-point  force  to  mini-manipulator 
end-point  position  in  the  lateral  direction  is  that  of  a  double  integrator  plant  with  an 
added  lightly  damped  torsional  resonance.  This,  in  addition  to  a  simple  lead  com¬ 
pensator,  constitutes  the  original,  unfavorable  configuration  of  the  system.  Figure  3.2 
shows  the  root-locus  for  the  “Unfavorable  Configuration”.  Notice  that  the  poles  associ¬ 
ated  with  the  torsional  resonance  immediately  move  into  the  unstable  right  half-plane. 
The  fact  that  the  pole  frequency  is  lower  than  the  zero  frequency  make  this  system 
inherently  difficult  to  control. 

On  the  other  hand,  if  the  pole  frequency  is  higher  than  the  zero  frequency,  then  one 
has  the  system  shown  in  Fig.  3.2  labeled  “Favorable  Configuration”.  Notice  that  here 
the  resonance  pole  moves  into  the  stable  left  half- plane. 
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Position  Control 


Figure  3.2:  The  Torsion  Problem:  Position  Control  Root-Locus 


Force  Control 


Figure  3.3:  The  Torsion  Problem:  Force  Control  Root- Locus 
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Similarly,  in  Fig.  3.3  we  see  the  “Favorable”  and  “Unfavorable”  pole-zero  configura¬ 
tions  for  end-point  force  control  of  the  mini-manipulator  using  integral  control.  When 
in  force  control,  the  plant  no  longer  has  a  rigid  body  mode.  The  pole  at  the  origin  is 
due  to  integral  control,  and  the  two  pairs  of  poles  and  zeros  on  the  imaginary  axis  are 
due  to  the  torsional  resonance. 

If  the  experimental  system  could  be  modified  to  cause  the  pole  frequency  to  be 
greater  than  the  zero  frequency,  the  tasks  of  both  position  and  force  control  would  be 
greatly  simplified.  This  possibility  will  now  be  investigated. 

3.3.2  A  Simplified  Model  of  the  Torsion  Mode 

A  simplified  model  of  the  flexible  arm  and  mini-manipulator  system  will  be  used  to 
analyze  the  torsion  mode.  Figure  3.4  depicts  this  simplified  model.  It  is  essentially  a 
view  of  the  flexible  arm  looking  from  the  end-point  towards  the  hub.  A  dextral  set  of 
mutually  perpendicular  unit  vectors  n2,  and  n3  are  fixed  in  inertial  reference  frame 
N.  Rigid  body  A,  whose  center  of  mass  is  located  at  C,  has  mass  M  and  central  inertia 
scalar  I  about  an  axis  parallel  to  n3.  Point  C  is  located  a  distance  h  along  the  n2 
direction  from  an  inertially  fixed  point  O.  A  dextral  set  of  mutually  perpendicular  unit 
vectors  at,  a2,  and  a3  are  fixed  in  a  reference  frame  attached  to  A.  A  linear  torsion 
spring  with  spring  constant  k  is  attached  to  A  at  C.  This  is  intended  to  approximate 
the  torsional  characteristics  of  the  flexible  arm.  A  point  mass  located  at  P  with  mass  m 
is  constrained  to  move  along  groove  G  which  is  parallel  to  ax  and  lies  a  distance  d  along 
the  a2  direction  from  C.  This  is  intended  to  approximate  the  mass  of  the  end-point  of 
the  mini-manipulator.  An  LED  located  at  L  lies  a  distance  l  in  the  a2  direction  from 
C.  It  is  connected  to  a  massless  rigid  link  which  is  connected  to  point  P.  A  contact 
point  T  lies  a  distance  b  in  the  —  a3  direction  from  C.  It  is  connected  to  a  massless  rigid 
link  which  is  connected  to  point  P. 

This  simplified  system  has  three  degrees  of  freedom  described  by  three  generalized 
coordinates:  qi,  g2,  and  93.  Generalized  coordinate  71  is  the  angle  between  unit  vectors 

and  Oj.  Generalized  coordinate  <j2  is  the  distance  from  0  to  C  along  the  nl  direction. 
Finally,  73  is  the  distance  from  C  to  T  along  the  ax  direction.  The  generalized  speeds 
are  defined  simply  as  u ,  =  7,  (*'  =  1, 2,3). 

Three  forces  contribute  to  the  motions  of  the  simplified  system.  An  internal  force 
Fax  is  exerted  on  P  by  body  A.  At  the  contact  point  T,  an  external  force  Rnx  is  exerted. 
Finally,  an  external  torque  -kqxa3  is  exerted  on  A  by  the  linear  torsion  spring. 

Given  this  system,  the  linearized  equations  of  motion  may  be  written  as 

-(/  +  md2)«i  +  md2U2  +  mdu3  +  Rb  -  kqx  =  0 

mdxii  —  ( M  +  m)u2  -  mu3  +  R  =  0  (3.1) 

mdu\  —  mu2  -  mua  +  F  +  R  =  0. 

When  T  is  in  contact  with  an  object  fixed  in  N,  the  following  condition  must  hold 
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Simplifying  and  linearizing  Eq.  (3.2)  yields 

bu  i  +  u2  +  u3  =  0. 


(3-3) 


w 


» 


3.3.2. 1  Position  Control 


When  T  is  not  in  contact  with  an  object,  as  is  the  situation  during  position  control, 
the  object  reaction  force  R  is  zero.  Making  this  substitution  and  taking  the  Laplace 
transform  of  Eqs.  (3.1)  yields 


— (/  +  mtPjs2  -  k 
rads1 
mds2 


Solving  Eq.  (3.4)  yields 


mds 2  mds2 
-(M  +  m)s2  -ms2 


—ms 


-ms 


’  ?l(s)  ' 
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92(5) 
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. «»(«) . 

-1 

F(s).  (3.4) 


9i(*) 

F(s) 

92(a) 

F(s) 


(3.5) 


Is2  4-  k 
-1 
Ms 2 

93(a)  [(£  +  TGr)/  +  d2]a2 +  *(£  +  w) 

F(s)  ~  s2(Is2  +  k) 

During  position  control,  the  position  of  the  LED  at  point  L  is  measured.  More  specifi¬ 
cally,  one  measures  a  quantity  y  given  by 


y  =  rOL 


n. 


=  g2  +  cos(?i)g3  -/sinfo) 
«  -Iqx  +  72  +  93- 

Subs  tituting  Eqs.  (3.5)  into  Eq.  (3.6)  yields 

(/+  md(d  -  /)]s2  +  k 


y(j) 

F(s) 


ms2(Is2  -I-  k) 


(3.6) 


(3.7) 


To  achieve  the  “Favorable  Configuration”  illustrated  in  Fig.  3.2,  where  the  frequency  of 
the  first  resonance  zero  is  less  than  that  of  the  first  resonance  pole,  Eq.  (3.7)  indicates 
that  the  following  condition  must  hold 


d(d  -  l)  >  0. 

This  can  be  met  by  one  of  the  two  following  conditions 

d  >  0  and  d  >  l 


or 


d  <  0  and  d  <  l. 


(3.8) 

(3.9) 
(3.10) 
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Enforcing  the  first  condition  is  not  easily  feasible.  By  examining  Fig.  3.4,  one  can  see 
that  the  first  condition  implies  that  the  LED  is  located  below  the  point  mass  P.  In  this 
configuration,  the  point  mass  would  most  likely  obscure  the  LED  from  the  overhead 
vision  system. 

The  second  condition  is,  however,  quite  feasible.  It  requires  that  the  point  mass  P  be 
located  below  the  center  of  mass  of  body  A ,  and  that  the  LED  be  above  the  point  mass 
P.  Thus,  we  have  an  easily  obtainable  system  configuration  that  produces  a  favorable 
pole-zero  pattern  for  position  control. 

3. 3. 2. 2  Force  Control 

When  in  force  control,  the  object  reaction  force  R  is  non-zero  and  the  condition  described 
in  Eq.  (3.3)  must  hold.  It  is  convenient  to  define  the  following  variables: 

*1  ^  -[I+md(d  +  b)] 

z2  =  m(d  +  6)  +  Mb  (3.11) 

Z3  =  m(d  +  b). 

Under  these  circumstances,  making  the  substitutions  indicated  above,  and  taking  a 
Laplace  transform,  Eq a.  (3.1)  become 

zis2  -  k 
z2s2 
z3s 2 

Solving  for  R(s)  yields 


By  examining  Eq.  (3.13),  achieving  a  “Favorable  Configuration”  requires  that 

/  +  m(d  +  6)J  <  /  +  md(d  +  b)  (3.14) 

or 

(d  +  b)2  <  d(d  -I-  b).  (3.15) 

Now  there  are  two  cases:  one  for  (d  4-  b)  <  0,  and  one  for  (d  +  6)  >  0.  For  each  case, 
Eq.  (3.15)  translates  into 


Case  1:  (d  +  b)  >  0 

Case  2:  (d  +  b)  <  0 

d  +  6  <  d 

d  +  6  >  d 

D- 

6  <  0 

b  >  0 

Thus,  there  are  two  possibilities  for  favorable  dynamics.  The  first  is  that 


0  b 
Ms 2  1 
0  1 


’  9i(«)  " 

0 

fc(«) 

= 

0 

.  w . 

-1 

F(s). 


(3.12) 


R(s)  _  k  —  z\s2 


F(s)  (*i  -  bz3)s2  -  k ' 


(3.13) 


(d  +  6)  >  0  and  b  <  0. 


(3.16) 
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However,  given  that  for  position  control  it  is  desirable  to  have  d  <  0,  Eq.  (3.16)  is 
impossible  to  satisfy.  The  second  possiblity  is  that 

(d  +  fc)  <  0  and  b>  0.  (3.17) 

But  for  d  <  0,  it  would  be  very  difficult  to  produce  a  physical  geometry  that  would 
satisfy  Eq.  (3.17).  Thus,  for  force  control,  the  “Unfavorable  Configuration”  is  practically 
unavoidable. 

3.3.3  Summary 

In  summary,  by  configuring  the  system  such  that  the  LED  is  situated  above  the  point  P , 
and  that  P  is  in  turn  below  C,  one  has  a  system  with  a  favorable  pole-zero  pattern  for 
position  control.  Unfortunately,  this  also  results  in  an  unfavorable  configuration  for  force 
control,  but  this  cannot  be  avoided.  In  physical  terms  of  the  actual  mini-manipulator, 
this  configuration  of  the  simplified  model  translates  into  the  following:  the  LED  should 
be  located  above  the  center  of  mass  of  the  mini- manipulator  end-point,  and  the  center 
of  mass  of  the  mini-manipulator  end-point  should  be  located  below  the  center  of  mass 
of  the  flexible  arm. 


3.4  Collocated  Control 

Collocated  control  is  a  type  of  control  where  the  sensors  and  actuators  are  located  at  the 
same  physical  position.  This  is  significant  because  it  results  in  robust,  stable  control. 
However,  it  has  the  drawbacks  of  not  yielding  good  end-point  accuracy  and  producing 
relatively  low  bandwidth  control.  Nevertheless,  there  are  times  when  it  is  appropriate 
to  use  collocated  control  with  the  flexible  arm  and  mini-manipulator  system. 

One  such  time  is  when  the  end-point  of  the  flexible  arm  or  the  mini-manipulator 
is  either  outside  the  field  of  view  of  the  optical  end-point  sensor,  or  occluded  from 
the  optical  end-point  sensor.  In  such  instances,  it  is  desirable  to  switch  from  high- 
performance,  end-point  control  to  the  more  dependable  collocated  control. 

Another  time  when  collocated  control  is  appropriate  is  when  the  desired  end-point 
position  is  outside  the  workspace  of  the  mini-manipulator.  Under  this  circumstance, 
it  is  desirable  to  use  a  collocated  mini-manipulator  controller  while  the  flexible  arm 
controller  moves  the  mini-manipulator  into  range.  During  this  motion,  the  collocated 
controller  holds  the  mini-manipulator  in  some  nominal  configuration.  Once  the  desired 
end-point  position  is  in  range,  the  mini-manipulator  switches  to  end-point  control. 

The  type  of  collocated  control  used  is  PD  —  Proportional  and  Derivative.  More 
specifically,  the  control  torques  for  each  of  the  three  actuators  may  be  described  by 

Th  =  ■fi'ff[*j»«(?ieomm„d-?i)  +  ^/f(0-u1)] 

Ta  —  Ka  [^p^(?n+  'command  ~  0n+l)  +  ^(^(O  —  Un+l)] 

Tb  =  Kb  [*pfl(?T»+Jcomm.nd  ~  9n+j)  +  kdB(0  -  U„+2)]  • 


(3.18) 
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Figure  3.5:  Step  Response  for  Collocated  Control  of  Flexible  Arm 

Here,  Th  is  the  hub  motor  torque.  Ta  and  Tb  are  the  two  mini-manipulator  control 
torques.  The  gereralized  coordinates  ft,  qn+i,  and  qn+ 2  correspond  to  the  hub  angle  of 
the  flexible  arm,  and  to  the  two  mini-manipulator  joint  angles  respectively.  The  time 
derivatives  of  ft,  qn+ 1,  and  qn+2  are  designated  t*i,  un+j,  and  un+2  respectively.  Since 
this  control  mode  does  not  require  high  performance,  the  selection  of  feedback  gains 
was  not  rigorously  optimized.  Instead,  gains  were  simply  selected  by  using  values  which 
experimentally  proved  to  yield  acceptable  performance. 

Figure  3.5  shows  the  response  of  the  flexible  arm  to  a  step  command  in  hub  angle 
position.  The  hub  angle  takes  just  over  2  seconds  to  make  a  20  degree  slew  with  very 
little  overshoot. 

Figure  3.6  shows  the  response  of  one  of  the  mini- manipulator  links  to  a  step  command 
in  link  angle.  The  slew  takes  approximately  0.2  seconds  and  has  negligible  overshoot. 
Notice  that  there  is  a  significant  delay  between  the  time  when  the  actuator  begins  to 
output  a  torque  and  when  the  link  begins  to  move.  This  is  due  primarily  to  brush 
friction  in  the  mini-manipulator  motors. 


3.5  Friction  Compensation 

As  described  in  Section  3.2,  the  hub  motor  is  brushless,  and  hence  adds  very  little  friction 
to  the  hub  of  the  flexible  arm.  On  the  other  hand,  the  mini-manipulator  motors  do  have 
brushes,  and  consequently  they  add  a  much  greater  amount  of  friction  to  the  two  base 
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Time  (sec) 


Figure  3.6:  Step  Response  for  Collocated  Control  of  Mini-Manipulator 

joints  of  the  mini-manipulator.  The  friction  level  present  in  the  two  mini-manipulator 
motors  is  roughly  0.007  Nm  -  about  5%  of  the  peak  torque  of  0.15  Nm,  and  as  much 
as  20%  of  the  torque  required  to  control  end-point  force  to  common  levels  of  about  35 
grams  force.  In  light  of  these  significant  levels  of  friction,  some  means  of  reducing  the 
effect  of  friction  on  the  system  was  sought. 

It  was  experimentally  determined  that  the  friction  present  in  the  motors  was  essen¬ 
tially  Coulomb  friction.  There  was  no  discernible  difference  between  static  and  kinetic 
friction  levels,  and  no  significant  viscous  friction.  Thus,  the  common  practice  of  using 
a  dither  signal  to  break-up  static  friction  is  not  effective  in  this  case. 

Another  type  of  friction  compensation  that  was  attempted  was  to  feed-forward  an 
estimate  of  the  Coulomb  friction  level  to  the  motors.  In  one  case,  this  estimate  was 
simply  the  absolute  value  of  the  Coulomb  friction  level  multiplied  by  the  sign  of  the 
corresponding  motor  shaft  angular  velocity.  More  specifically, 

Ta  =  .Fsgn(un+1)  (3.19) 

Tb  =  .Fsgn(un+2).  (3.20) 

where  Ta  and  TB  are  the  two  mini-manipulator  motor  torques,  T  is  the  absolute  value 
of  the  Coulomb  friction  level,  and  tin+i  and  un+2  are  the  two  motor  shaft  angular 
velocities.  This  scheme  of  friction  compensation  has  a  very  noticeable  drawback:  when 
the  angular  velocities  are  small,  the  friction  compensation  torques  tend  to  “chatter” 
(i.e.  oscillate  very  rapidly)  unless  T  is  made  very  small.  This  chatter  can  be  minimized 
by  the  addition  a  dead-band  and/or  hysteresis. 
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However,  what  experimentally  proved  to  yield  the  best  friction  compensation  was 
the  use  of  linear,  angular  velocity  feed-forward  with  limits.  Under  this  scheme,  the 
friction  compensation  torques  may  be  described  by 


T.  -  | 

r  few- 

il*n+l|  <  U,im 

l  T 

|«»+l|  >  «/.m 

TB  -  1 

r  (=£)“"« 

l“n+a|  <  «Km 

i  r 

|«n+2|  >  Uf.m 

(3-21) 

(3.22) 


This  form  of  friction  compensation  was  used  only  while  the  mini-manipulator  was  under 
end-point  position  control.  It  would  also  have  been  desirable  to  use  some  kind  of  friction 
compensation  during  force  control,  but  none  of  the  above  methods  yielded  satisfactory 
results  —  the  primary  problem  was  that  they  all  tended  to  destabilize  the  torsion  mode. 


3.6  Identification  of  the  Flexible  Arm 

As  was  demonstrated  in  Section  3.4,  achieving  satisfactory  collocated  control  of  the 
flexible  arm  does  not  require  an  accurate  model  of  the  system.  The  price  one  pays  for 
this  is  poor  performance.  Achieving  high-performance  end-point  control  of  the  flexible 
arm,  on  the  other  hand,  requires  a  fairly  accurate  description  of  the  flexible  arm’s  rigid 
body  and  bending  modes. 

Since  the  whole  point  in  having  a  mini-manipulator  at  the  end  of  the  flexible  arm 
is  to  produce  much  higher  performance  than  possible  with  the  flexible  arm  alone,  it  is 
reasonable  and  very  generic  to  assume  that  the  controllers  for  the  two  systems  will  be 
bandwidth  separated.  This  spectral  separation  allows  one  to  design  the  two  controllers 
independently.  Thus,  in  terms  of  the  flexible  arm,  it  is  only  necessary  to  identify  transfer 
functions  from  the  hub  motor  input  to  the  two  flexible  arm  outputs:  end-point  position 
and  hub  angle. 

3.6.1  Experimental  Procedure 

One  way  to  identify  the  flexible  arm  is  through  the  use  of  sine  sweeps.  That  is,  inputting 
a  sinusoidally  varying  torque  to  the  hub  motor,  and  measuring  the  relative  magnitude 
and  phase  of  the  end-point  position  and  hub  angle  for  a  wide  range  of  frequencies. 

A  frequency  response  analyzer  was  used  to  provide  the  sinusoidal  input,  and  to 
calculate  the  relative  magnitude  and  phase  of  the  outputs.  The  input  frequency  was 
varied  from  as  low  as  practically  possible,  0.1  Hz,  to  a  point  where  the  bending  mod-’ 
resonances  became  insignificantly  small,  14.0  Hz. 

The  experimental  transfer  function  data  taken  with  the  frequency  response  analyzer 
was  then  fit  to  two  model  transfer  functions:  one  for  the  transfer  function  from  hub 
torque  to  hub  angle,  and  the  other  for  the  transfer  function  from  hub  torque  to  flexible 
arm  end-point  position.  This  fit  was  done  using  a  numerical  conjugate  gradient  search 
algorithm  to  minimize  a  cost  function. 
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3.6.2  A  Cost  Function  for  the  Transfer  Function 

Let  G(s)  represent  the  actual  transfer  function  we  wish  to  identify,  and  let  G(s)  represent 
our  estimate  of  (7(s).  We  then  wish  to  minimize  some  measure  of  the  difference  between 
G(s)  and  G($).  This  has  to  be  done  at  each  of  the  discrete  frequency  values  s:  (j  = 
l,...,n)  where  G(s)  was  measured.  A  typical  measure  to  minimize  is  the  square  error 
given  by 

J2  =  ^2  {^(si)  -  •  (3.23) 

i=l 

This  cost  function,  however,  has  the  disadvantage  of  being  heavily  influenced  by  large 
errors.  A  cost  function  that  is  less  sensitive  to  large  errors  is  one  that  is  based  on  the 
absolute  value  of  the  error. 

7i  =  f;|G(sl)-C(s,)|.  (3-24) 

isl 

This  is  the  measure  that  was  minimized  to  obtain  an  “optimal”  estimate  of  G(s). 

3.6.3  Experimental  Model  of  the  Flexible  Arm 

As  has  been  shown  in  the  past,  a  truncated  modal  form  may  be  used  to  approximate 
the  transfer  function  of  the  system.  Since  no  resonances  beyond  the  third  bending  mode 
were  experimentally  observed,  only  the  first  three  modes  were  modeled.  The  transfer 
function  from  hub  torque  to  hub  angle  may  be  approximated  as 

?i(3)  _  rgo  ,  f _ TJi _ 

Th{s)  s(s  +  aH)  ££  \  s2  +  2  (Vis  +  uf 

The  first  term  in  Eq.  (3.25)  refers  to  the  rigid  body  mode  of  the  flexible  arm.  The 
remaining  three  terms  describe  the  first  three  bending  modes  of  the  flexible  arm.  Simi¬ 
larly,  the  transfer  function  from  hub  torque  to  the  end-point  position  of  the  flexible  arm 
may  be  approximated  as 

WeA*)  (  & Jwtx  \  /  Twexo  ,  f  Twex% 

Th\»)  ~  \s  +  afuiec )  \s(s  +  aH)  “  \  s2  +  2(^3  +  w? 

Equation  (3.26)  has  a  leading  factor  not  present  in  Eq.  (3.25).  This  leading  factor 
reflects  the  presence  of  a  10  Hz  analog  low-pass  filter. 

The  parameter  an  represents  the  amount  of  viscous  damping  present  in  the  hub 
motor,  and  was  experimentally  determined  by  examining  the  rate  of  decay  of  the  rigid 
body  mode.  The  parameter  awex  represents  the  known  cutoff  frequency  of  the  low-pass 
filter  for  the  flexible  arm  end-point  position  sensor.  Another  parameter,  that  will  be 
discussed  more  later,  is  a/u\.  It  represents  the  known  cutoff  frequency  of  the  pseudo- 
differentiator  that  was  used  to  derive  angular  rate  information  from  measurements  of 
the  hub  angle.  The  other  parameters  in  Eqs.  (3.25)  and  (3.26),  namely  r7i,  rweit,  C., 


(3.25) 
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Hub  Motor  Damping 

Filter  ( 

afwe  x 

Dutoff  Frequencies 
a/ui 

0.15  rad/sec 

10  Hz 

10.27  Hz 

Mode 

Residue 

Frequency  (Hz) 

Damping  Ratio 

i 

r*  _ 

rwes< 

Ui 

0 

0 

0.2166 

-2.6863 

0 

— 

1 

0.4219 

2.1429 

2.60 

0.0298 

2 

8.2046 

-2.1077 

4.85 

0.0153 

3 

0 

2.4795 

10.40 

0.0248 

Table  3.1:  Values  of  the  Flexible  Arm  Model  Parameters 

and  were  determined  by  minimizing  the  cost  function  J\.  The  numerical  values  for 
all  of  these  parameters  are  tabulated  in  Table  3.1. 

Using  Eqs.  (3.25)  and  (3.26),  and  the  values  listed  in  Table  3.1,  a  numerical  theoret¬ 
ical  model  of  the  flexible  arm  was  constructed.  Figures  3.7  and  3.8  show  the  Bode  plots 
of  the  flexible  arm  transfer  functions.  One  trace  shows  the  actual  experimental  values 
obtained  using  the  frequency  analyzer,  and  the  other  trace  shows  the  values  obtained 
using  the  mathematical  model. 

Looking  at  the  transfer  function  from  hub  torque  to  hub  angle,  one  notices  that  there 
is  an  alternating  pole-zero  pattern.  Starting  from  the  low  end  of  the  frequency  scale  and 
moving  to  the  right,  one  encounters  first  a  zero,  then  a  pole,  a  zero,  and  then  another 
pole  —  basically,  two  poles  and  zeros.  The  third  bending  mode  makes  no  significant 
appearance  in  this  transfer  function. 

In  contrast,  the  transfer  function  from  hub  torque  to  flexible  arm  end-point  position 
has  three  discernible  poles,  and  no  discernible  zeros.  This  apparent  lack  of  zeros  arises 
because  the  zeros  of  the  transfer  function  are  not  located  near  the  imaginary  axis  in 
the  8-plane.  In  fact,  some  of  the  zeros  actually  lie  in  the  right  half-plane.  It  is  this 
non-minimum  phase  characteristic  of  the  non-collocated  transfer  function  that  makes 
good  end-point  control  so  difficult  to  achieve. 


3.7  End-Point  Control  Design  for  the  Flexible  Arm 

3.7.1  State  Space  Model 

For  purposes  of  control  law  design,  it  is  convenient  to  recast  the  equations  describing 
the  theoretical  model  of  the  flexible  arm,  namely  Eqs.  (3.25)  and  (3.26),  into  a  state 
space  representation.  In  such  a  form,  the  equations  of  motion  of  the  system  may  be 
described  simply  as 


x 

y 


Fx  -(-  Gu 
Hx. 


(3.27) 
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Using  a  modal  representation,  one  can  arrive  at  the  following  block  diagonal  form  for 
the  system  dynamics  matrices 

r°  1  i 

0  -OH 


Fn  — 


—  2Ciu>i 


0  1 
—U>2  -2(2U>2 


0  1 
-a>|  -2(3u>3 


(3.28) 


afwex  hwex  fl/ti 

O/ul hul  0 


-a/ui 


G  =  [OIOIOIOIOO] 

H  o  0  0  0  0  0  0  0  1  o' 
0000000001 


(3.29) 


(3.30) 

(3.31) 


(3.32) 

(3.33) 

(3.34) 


The  quantities  Wex„  and  Uim  are  simply  the  filtered  measurements  of  Wex  and  ui 
respectively.  These  two  filter  transfer  functions  are  given  by 


where 


*  =  [  90  ft  ?1  9l  ?2  ft  ?3  ?3  Wex 

«  =  Th 

V  =  f^-1. 


afwex 

S  +  O/u/er 


^Exm  _  f  af‘wtx  \ 
Wex  \s  +  afwex) 

nim  _  f  \ 

Ui  “  \s  +  afwex)' 


The  output  matrices  hwex  and  hul  are  defined  by 

^Ex  = 

Ul  =  hulx 


where 


—  |  rwexo  0  rweX]  0  rwex2  0  rwexj  0  0  0  j 

=  [  0  r,lo  0  r?ll  0  r?lj  0  rqli  0  0  ]  . 


(3.35) 

(3.36) 

(3.37) 

(3.38) 

(3.39) 

(3.40) 
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Since  the  flexible  arm  is  to  be  controlled  by  a  digital  computer,  it  is  necessary  to  have 
a  discrete  time  model  of  the  system.  The  continuous  time  system  dynamics  described  by 
Eqs.  (3.27)  may  be  transformed  via  a  zero  order  hold  into  a  discrete  time  representation 
given  by 


*n+l  =  +  run 

Vn  =  Hxn.  (3.41) 


3.7.2  Optimal  Regulator  Design 

An  “Optimal  Regulator”  is  a  type  of  controller  that  minimizes  some  cost  function.  The 
standard  formulation  is  to  minimize  the  cost  J  given  by 


J  = 


1  N 
1  n=0 


(3.42) 


subject  to  the  constraint 


*n+l  = 


(3.43) 


Rxx  and  Ruu  are  symmetric  weighting  matrices  on  the  states  and  the  controls,  selected 
to  yield  a  desired  performance.  Since  the  quantity  that  is  directly  trying  to  be  controlled 
is  Wex  ,  it  is  reasonable  to  choose  Rxx  such  that  there  is  a  cost  associated  with  non-zero 
values  of  Wex-  This  may  be  done  by  choosing 


D  _  lT  L 

■n'rrt vex  nwexutvex‘ 


(3.44) 


The  damping  in  the  system  was  enhanced  by  adding  costs  to  the  elements  of  Rxxwex  cor¬ 
responding  to  each  of  the  modes  of  the  system.  In  this  case,  the  following  modifications 
to  the  basic  Rxxwex  given  in  Eq.  (3.44)  were  made: 


Hxxvtet r(2,  2) 

4= 

■f2rx«/ex(2,  2)  -f-  0.2 

Rxxu iex(4, 4) 

<= 

Hxxv/ex(4, 4)  +  1.0 

Rxxwex(6i 0) 

4= 

Xxxwcx(6,6)  +  10.0 

(3.45) 

Rxxwex{&i  8) 

4= 

Rxxwex(8,  8). 

Using  a  control  cost 


Rvuivex  =  0.0025 


(3.46) 


the  cost  function  given  by  Eq.  (3.42)  was  minimized  to  obtain  the  optimal  feedback 
control  law 

^n  ^  wexi^  command  ~  *n)  (0-47) 

where  ^command  is  the  desired  state  vector  and  Kwex  is  the  feedback  gain  matrix.  The 
actual  numerical  values  of  the  feedback  gains  are 


Kwex  =  [  35.71  12.34  -11.63  11.65  622.01  35.41  1.79  0.02  0  0  ].  (3.48) 
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3.7.3  Steady-State  Optimal  Estimator  Design 

Since  not  all  of  the  states  are  available  for  measurement,  a  steady-state  optimal  estimator 
was  used  to  estimate  the  state  vector.  For  a  discrete  time  system  given  by 

*„+l  =  **n+  run+  rxWn  (3.49) 

yn  =  Hxn  +  t>n  (3.50) 

where  the  process  noise  wn  and  measurement  noise  vn  may  be  characterized  by 


£{“4  =  =  0 

(3.51) 

=  £{vivj}  =  0  if  i/j 

(3.52) 

and 

(3.53) 

£{»„»!)  =  Q„ 

(3.54) 

then  it  is  desired  to  choose  L  such  that 

*n+l  =  *K  +  r*n 

(3.55) 

*n+l  =  *n+l  +  ^(Vn+l  ~  ^*n+l) 

(3.56) 

yields  an  “optimal”  estimate  x  of  the  true  state  x.  The  actual  values  of  the  process  noise 
and  measurement  noise  covariances  were  not  determined.  Instead,  Qxx  and  were 
treated  as  design  parameters  to  arrive  at  an  estimator  that  had  satisfactory  performance. 
In  choosing  Qxx  and  there  is  a  trade-off  between  the  “speed"  of  the  estimator  error 
roots,  and  the  “noisiness”  of  the  state  estimate.  Basically,  the  noise  covariance  matrices 
were  chosen  to  yield  the  fastest  possible  estimator  error  roots,  while  providing  no  more 
noise  in  the  output  estimate  than  was  actually  measured.  Using  the  values 

=  rrT  (3.57) 

Qn™  =  V  10°4  <358) 

■ 


the  estimator  gain  matrix  was  found  to  be 


-o.n 

-o.oo 


—0.74 

0.04 


-0.01 

0.00 


0.3S  -0.00  0.06  0.00  -0.06  0.25 

0.06  0.00  0.06  0.00  -0.02  0.00 


Now,  based  on  measurements  of  Wex„  and  uim,  the  state  vector  *  may  be  estimated 
using  the  current  estimator  equations  given  in  Eqs.  (3.55)  and  (3.56). 
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Figure  3.9:  End-Point  Control  Step  Response  for  the  Flexible  Arm 

3.8  End-Point  Control  of  the  Flexible  Arm 

Using  the  optimal  regulator  and  estimator  designed  in  Section  3.7,  end-point  position 
control  of  the  flexible  arm  is  now  possible.  Running  the  controller  at  a  sample  rate  of 
100  Hz,  the  flexible  arm  was  given  a  step  command  in  end-point  position.  Figure  3.9 
shows  the  time  history  of  the  end-point  of  the  flexible  arm  and  the  control  torque  that 
was  used.  One  will  notice  that  there  is  a  roughly  50  msec  (5  sample  period)  delay  from 
the  time  the  step  command  is  given  to  the  time  the  flexible  arm  end-point  begins  to 
move.  This  represents  the  time  required  for  a  bending  wave  to  propagate  down  the 
length  of  the  flexible  arm.  The  commanded  slew  of  just  over  5  cm  was  accomplished  in 
0.65  seconds  with  negligible  overshoot.  This  compares  quite  favorably  with  the  period 
of  the  first  cantilever  mode  of  the  flexible  arm  which  is  1.4  seconds.  This  step  resulted  in 
a  peak  control  torque  of  approximately  the  same  size  as  the  maximum  torque  available. 

3.9  Identification  of  the  Mini-Manipulator 

Now  that  the  flexible  arm  is  under  end-point  position  control,  there  remains  the  task 
of  dosing  both  a  position  and  force  control  loop  around  the  end-point  of  the  mini¬ 
manipulator.  To  do  this,  we  must  have  some  description  of  the  dynamics  of  the  mini- 
manipulator.  Figure  3.10  depicts  a  close-up  view  of  the  mini- manipulator.  One  will  note 
that  in  terms  of  the  actuator  torques  applied  at  points  Xa  and  Xb ,  and  the  Cartesian 
position  of  the  end-point  Ag,  this  is  a  highly  non-linear  mechanism.  It  does,  however, 
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Figure  3.10:  A  Close-Up  View  of  the  Mini-Manipulator 

have  the  advantage  of  being  very  elegant  mechanically:  it  is  all  direct  drive,  and  the 
motors  are  not  required  to  accelerate  heavy  masses  such  as  other  motors. 

One  strategy  that  is  used  in  controlling  non-linear  systems  involves  having  a  knowl¬ 
edge  of  the  governing  equations  of  motion,  and  using  this  to  make  the  non-linear  system 
behave  like  a  desired  simple  linear  system.  This  is  known  as  computed  torque.  Since 
the  full  equations  of  motion  of  the  mini-manipulator  are  far  too  complex  to  be  dealt 
with  in  real  time  with  available  computer  power,  some  other  means  of  addressing  the 
control  problem  must  be  found. 

For  a  mini-manipulator  that  has  linkages  which  are  light  compared  to  the  end¬ 
point  mass,  the  equations  of  motion  may  be  approximated  by  a  much  simpler  set  of 
equations.  All  of  the  dynamic  non-linear  terms  in  the  governing  equations  of  motion 
are  associated  with  the  dynamics  of  the  linkages.  Since  in  this  case  the  linkages  are 
relatively  lightweight,  the  non-linear  dynamic  terms  in  the  equations  of  motion  may  be 
neglected.  Thus,  one  is  left  only  with  kinematic  non-linearities  and  a  greatly  simplified 
set  of  equations  which  approximate  the  full  equations  of  motion  of  the  mini- manipulator. 


3.9.1  The  Mini-Manipulator  Jacobian 

One  can  relate  the  variation  in  end-point  position  as  seen  in  the  mini-manipulator 
reference  frame  A  to  the  variation  in  joint  angles  via 


(3.60) 


where  Rs  and  Ry  are  the  Cartesian  coordinates  of  the  end-point  Xg  in  reference  frame 
A.  The  2x2  matrix  J  is  known  as  the  Jacobian.  In  general,  Eq.  (3.60)  may  be  written 

as 


6x  =  J  Sq. 


(3.61) 
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If  one  considers  the  case  of  static  equilibrium,  the  principle  of  virtual  work  may  be  ap¬ 
plied  to  the  mini-manipulator.  If  T  is  a  vector  whose  elements  are  the  joint  torques,  and 
F  is  a  vector  whose  elements  are  the  forces  exerted  by  the  end-point  on  the  environment, 
then  the  principle  of  virtual  work  may  be  stated  as 

TT6q  -  Ft6x  =  0.  (3.62) 


Substituting  Eq.  (3.62)  into  Eq.  (3.61),  one  arrives  at  the  expression 


T  =  JT  F, 


(3.63) 


or  more  specifically 


'  Ta  ' 

=  JT 

‘  h  ‘ 

TB 

fi 

(3.64) 


Thus,  given  a  desired  applied  force,  Eq.  (3.64)  allows  one  to  calculate  the  joint 
torques  necessary  to  provide  that  force.  In  essence,  we  can  make  the  system  appear  to 
behave  as  if  a  force  f\ax  -f  f2a2  is  acting  on  the  end-point  using  only  control  torques  Ta 
and  Tb .  One  must  remember  that  this  relationship  holds  exactly  only  when  the  base 
link  of  the  mini-manipulator  is  inertially  fixed.  However,  actual  practice  has  shown 
that  for  the  relatively  slow  motions  of  the  flexible  arm,  Eq.  (3.64)  is  still  a  very  good 
approximation. 


3.9.1. 1  Evaluating  the  Jacobian 

Referring  to  Fig.  3.10,  the  sine  and  cosine  of  the  angle  g„+4,  represented  by  cn+4  and 
5n+4,  may  be  found  from  known  quantities  via 


Cl 

ca 

»a 

Sy 

<h 

Cn+4 

«n+4 


y/[lo  +  fl(Cn+l  -  Cn+3)]J  +  [fl(^n+l  -  -»n+a)]2 

iL 
2  h 


— (5n+2  ~  Sn+l) 
ei 


yrr 


$OfC*y  CqS. y 


Then,  the  following  sequence  of  equations  determine  the  Jacobian: 


dt\ 

#9n+l 

dqn+2 


~  [fi(sn+iCn+2  -  Cn+iSn+2)  -  /o/l«n+l] 
~  [fi(Cn+ian+2  -  Sn+lCn+2)  +  Wl-Sn+2] 


(3.65) 

(3.66) 

(3.67) 

(3.68) 

(3.69) 

(3.70) 

(3.71) 


(3.72) 

(3.73) 
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Jn  = 
J\2  = 

Jjl  ss 

Jn  - 


#7 

#?n+l 

dqn+2 

da 

dgn+i 

da 

dqn+ 2 

dgn+4 

d<In+l 

dqn+ 4 
dqn+2 
dRx 
dqn+i 
dRx 
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dRy 

dqn+1 
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dqn+i  dqn+ 1 
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=  ~hsn+4 


#?n+ 1 


=  -^l«n+2  ~  h*n+4 


—  hcn+4 


dqn+4 

dqn+l 


dqn+4 

dqn+ 2 


-  ^lcn+2  +  hcn+4 


dqn+4 

dqn+i 


The  Jacobian  is  then 


J  = 


Jn  Ju 
J21  J22 


(3.74) 

(3.75) 

(3.76) 

(3.77) 

(3.78) 

(3.79) 

(3.80) 

(3.81) 

(3.82) 

(3.83) 

(3.84) 


3.9.2  Experimental  Identification 

With  the  Jacobian  and  the  assumption  of  relatively  lightweight  linkages,  we  now  have  a 
means  of  experimentally  identifying  the  dynamics  of  the  mini-manipulator.  The  general 
idea  is  to  input  a  desired  sinusoidally  varying  end-point  force,  and  to  measure  the 
resulting  position  of,  and  force  at,  the  mini-manipulator  end-point. 


3.9.2. 1  Position  Transfer  Function 


Using  the  frequency  response  analyzer,  a  sinusoidal  signal  s(t)  was  generated.  This  was 
then  used  to  generate  two  desired  end-point  force  vectors  given  by 


(3.85) 

(3.86) 


One  may  then  substitute  these  desired  force  vectors  into  Eq.  (3.63)  to  yield  two  sets  of 
mini- manipulator  joint  torques 


(3.87) 
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T  = 
■L  v 


jtf , 


(3.88) 

Then,  while  applying  joint  torque  Tx,  one  uses  the  frequency  analyzer  to  measure  the 
lateral  position  of  the  mini-manipulator  end-point  Rx  and  to  determine  the  relative 
magnitude  and  phase.  This  determines  the  transfer  function  from  lateral  force  to  lat¬ 
eral  position,  or  Rx(s)/f i«*(s).  In  a  similar  fashion,  by  applying  joint  torque  Ty  and 
measuring  Ry,  one  arrives  at  the  transfer  function  from  longitudinal  force  to  longitudinal 
position,  or  Ry(s)/ fu(s). 

Again,  using  a  truncated  modal  form,  the  experimental  frequency  response  data  was 
fit  to  a  model  described  by 


R*(s) 

(  afp  ) 

r  n 

fui •) 

\3  +  afp) 

Rvis) 

(  afv  ) 

r  --  i) 

M*) 

\S  +  afp) 

[  s2  +  2Cr »,WryiS  +  U)}y.  J  j 

(3.89) 


(3.90) 


It  should  be  noted  that  although  the  mini-manipulator  represents  a  highly  non-linear 
device,  we  are  mapping  the  mini-manipulator  dynamics  through  the  Jacobian  and  at¬ 
tempting  to  fit  the  result  to  a  linear  model.  The  leading  factors  in  Eqs.  (3.89)  and 

(3.90)  reflect  the  presence  of  a  40  Hz  analog  low-pass  filter.  As  before,  the  cost  function 
J\  given  in  Eq.  (3.24)  was  minimized  to  yield  optimal  estimates  of  the  parameters  rrx., 
TrV a  Wrij,  uryi ,  Cr*. ,  and  Qryi.  In  the  lateral  direction,  the  experimental  data  was  fit  to 
Eq.  (3.89)  for  mTX  =  1.  The  reason  for  using  mTX  =  1  is  that  experimental  evidence 
showed  that  only  the  rigid  body  and  torsion  mode  were  significant.  In  the  longitudinal 
direction,  the  experimental  data  was  fit  to  Eq.  (3.90)  for  mTy  =  0.  Here,  the  reason 
for  only  considering  the  rigid  body  mode  is  that  bending  and  torsion  in  the  flexible 
arm  affect  motions  primarily  only  in  the  lateral  direction.  The  numerical  values  of  the 
parameters  obtained  from  the  optimization  are  listed  in  Table  3.2. 

Using  Eqs.  (3.89)  and  (3.90),  and  the  values  listed  in  Table  3.2,  a  numerical,  theo¬ 
retical  model  of  the  mini-manipulator  position  dynamics  was  constructed.  Figures  3.11 
and  3.12  show  the  Bode  plots  of  the  mini-manipulator  position  transfer  functions.  As 
before,  one  trace  shows  the  actual  experimental  values  obtained  using  the  frequency  re¬ 
sponse  analyzer,  and  the  other  trace  shows  the  values  obtained  using  the  mathematical 
model. 


Examining  the  transfer  function  from  lateral  end-point  force  to  position,  or  Rx(s)/ 
one  can  see  that  the  two  mode  linear  model  does  a  reasonable  job  of  fitting  the  experi¬ 
mental  data.  One  will  note  that,  as  predicted  in  Section  3.3,  the  zero  frequency  is  less 
than  the  pole  frequency.  This  results  in  “favorable”  dynamics  that  make  the  system 
easy  to  control.  The  single  pole  seen  in  Fig.  3.11  is  due  to  the  torsion  mode.  The  bend¬ 
ing  modes  in  the  flexible  arm  have  only  a  negligible  impact  on  the  frequency  response 
—  their  presence  being  seen  in  the  slight  differences  between  experiment  and  model  at 
about  3  Hz  and  8  Hz. 

The  transfer  function  from  longitudinal  end-point  force  to  position,  or  Rv{s)/f2(s)y 
looks  just  like  a  l/s2  plant.  Again,  the  single  mode  linear  model  does  a  reasonable 


Phase  (deg)  Magnitude 
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Filter  Cutoff  Frequency 

_ ILe _ 

40  Hz 


Lateral  Direction  (mrx 

=  1) 

Mode 

Residue 

Frequency  (Hz) 

Damping  Ratio 

i 

Trxi 

Wr*,- 

Crx, 

0 

85.0834 

— 

— 

1 

19.3234 

11.40 

0.0385 

I  Longitudinal  Direction  (mry  =  0) 

Mode 

i 

Residue 

rrvi 

Frequency  (Hz) 

“ry. 

Damping  Ratio 
Cry, 

0 

120.7804 

— 

— 

Table  3.2:  Values  of  the  Mini-Manipulator  Model  Parameters:  Position 


Figure  3.11:  Bode  Plot  for  Mini-Manipulator  Position  Rx 
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Figure  3.12:  Bode  Plot  for  Mini-Manipulator  Position  Ry 


job  of  fitting  the  experimental  data.  The  slightly  larger  phase  lag  in  the  experimental 
model  may  be  attributed  to  time  delays  in  the  digital  computer  that  was  used  during 
the  identification  process. 


3.9.2. 2  Force  Transfer  Function 


To  identify  the  force  transfer  functions,  the  mini-manipulator  end-point  was  brought  into 
contact  with  an  inertially  fixed  object.  As  in  identifying  the  position  transfer  functions 
of  the  mini-manipulator,  the  frequency  response  analyzer  was  used  to  generate  the 
two  desired  end-point  force  vectors  given  by  Eqs.  (3.85)  and  (3.86).  In  this  case,  the 
sinusoidally  varying  desired  force  vectors  contained  a  DC  bias  level  to  ensure  that  the 
mini-manipulator  end-point  remained  in  contact  with  the  object.  However,  now  instead 
of  measuring  the  end-point  position,  the  end-point  force  wa^  "asured.  The  lateral 
force  f\  and  the  longitudinal  force  were  measured,  thereby  rmining  the  transfer 
functions  fi(s)/fu(s)  and  h{s)/ f2d{s). 

The  truncated  modal  form  used  to  fit  the  experimental  data  was 


/»(*) 

fu(s) 

m 

fu(») 


B(s) 

B(s) 


_ I fh _ 

•s2  +  2C/iiw/iia  +  u}\, 

_ rJ2i _ 

s2  +  2C/a,w/2js  +  wJ2, 


(3.91) 

(3.92) 
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Bessel  Type  Filter 

bfn 

5.9530  x  107 

DC  Gain 

1 

bfd2 

819.62 

Pole  1 

-151.62 

bJd3 

4.9391  x  10s 

Pole  2 

-334.00  +  530.16; 

bfd4 

5.9530  x  107 

Pole  3 

-334.00  -  530. 16j 

Lateral  Direction  (m/i 

=  1) 
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Frequency  (Hz) 

Damping  Ratio 

i 

rfU 

w/ii 

C/i. 

0 

3.6444 

— 

1 

7619.3 

10.72 

0.0568 

Lateral  Direction  (m/i 

=  3) 

Mode 

Residue 

Frequency  (Hz) 

Damping  Ratio 

t 

rfh 

w/i, 

C/i< 

0 

2.9885 

— 

— 

1 

-237.01 

3.46 

0.0501 

2 

1015.2 

8.85 

0.0694 

3 

7154.6 

10.72 

0.0595 

Longitudinal  Direction  (m/2  =  0) 

Mode 

i 

Residue 

r/2. 

Frequency  (Hz) 

“>/2i 

Damping  Ratio 

C/2. 

0 

5.4385 

— 

Table  3.3:  Values  of  the  Mini-Manipulator  Model  Parameters:  Force 
where  B(s)  is  a  third  order  Bessel  type  filter  given  by 

■  <3-93) 

Minimizing  the  cost  function  J\  given  in  Eq.  (3.24),  the  force  transfer  function  param¬ 
eters  were  determined  for  both  mj\  =  1  and  mji  =3,  and  for  m/2  =  0.  The  numerical 
values  of  the  parameters  are  listed  in  Table  3.3. 

Using  Eqs.  (3.91)  and  (3.92),  and  the  values  listed  in  Table  3.3,  a  numerical  theoret¬ 
ical  model  of  the  mini-manipulator  force  dynamics  was  constructed.  Figures  3.13  and 
3.14  show  the  Bode  plots  of  the  mini-manipulator  force  transfer  functions. 

For  the  transfer  function  from  lateral  end-point  force  to  measured  force,  or  }\{s)j  fu{s), 
one  sees  bode  plots  of  the  experimental  system,  the  single  pole  model,  and  the  three  pole 
model.  The  three  pole  model  does  a  reasonably  good  job  of  matching  the  experimental 
bode  plot.  The  single  pole  model  does  not  do  as  good  a  job  of  modeling  some  of  the 
lesser  resonances,  however  it  does  do  well  at  modeling  the  major  torsion  resonance  at 
10.72  Hz.  Experimentation  has  shown  that  using  the  three  pole  model  does  not  produce 
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Approximately  Linear  System 


Figure  3.15:  Identification  by  Approximate  Linearization 


significantly  better  closed-loop  performance  than  the  single  pole  model.  For  this  reason, 
the  simpler  single  pole  model  was  used. 

In  the  frequency  range  of  interest,  from  DC  to  about  10  Hz,  one  can  see  from 
Fig.  3.14  that  the  transfer  function  from  longitudinal  end-point  force  to  measured  force, 
or  h(s)/ /w('S),  is  almost  a  pure  gain  —  no  dynamics.  Thus,  for  control  purposes,  a 
zero  pole  model  is  adequate. 

3.9.3  Approximate  Linearization  Via  the  Jacobian 

The  mini-manipulator  represents  a  highly  non-linear  dynamic  system.  In  general,  iden¬ 
tifying  such  a  non-linear  system  would  involve  measuring  all  of  the  mass,  stiffness,  and 
damping  properties  of  the  flexible  arm  and  mini-manipulator,  and  using  these  measured 
values  in  the  very  complicated  equations  of  motion  that  describe  the  system  dynamics. 
However,  as  demonstrated,  for  a  mini-manipulator  that  has  lightweight  linkages  and 
most  of  its  mass  concentrated  at  the  end-point,  this  highly  non-linear  system  may  be 
“mapped”,  via  the  Jacobian,  into  an  approximately  linear  system. 

Using  the  Jacobian,  one  can  calculate  the  joint  torques  necessary  to  provide  a  desired 
end-point  force  for  the  case  of  static  equilibrium.  This  is  done  using  Eq.  (3.63).  If, 
however,  the  linkages  are  lightweight,  and  the  end-point  mass  is  relatively  large,  the 
joint  torques  calculated  from  Eq.  (3.63)  approximate  the  torques  necessary  to  make  the 
mini-manipulator  end-point  behave  as  if  it  is  a  point  mass  acted  upon  by  the  desired 
force  —  a  linear  system.  Figure  3.15  shows  a  graphical  illustration  of  the  identification 
procedure. 

Intuitively,  the  reasoning  behind  why  the  Jacobian  can  be  used  to  map  such  a  non¬ 
linear  system  into  an  approximately  linear  one  is  as  follows.  The  full  equations  of  motion 
contain  many  non-linear  terms.  Some  of  these  are  related  to  dynamic  effects,  and  others 
are  related  to  purely  kinematic  effects.  As  the  linkages  of  a  manipulator  become  more 
lightweight,  and  the  end-point  mass  becomes  larger,  the  dynamic  non-linearities  become 
insignificant.  Thus,  one  is  left  primarily  with  equations  of  motion  that  contain  kinematic 
and  dynamic  linear  terms,  and  purely  kinematic  non-linear  terms.  However,  these  non¬ 
linear  kinematic  terms  are  exactly  what  the  Jacobian  describes,  and  using  the  Jacobian, 
one  can  essentially  subtract  them  out  of  the  equations  of  motion.  This  method  of 
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linearizing  the  mini-manipulator  dynamics  is  generic,  and  can  be  applied  to  all  systems 
that  have  relatively  lightweight  linkages,  relatively  large  end-point  masses,  and  a  square 
Jacobian. 


3.10  Mini-Manipulator  Control  Design 

As  with  the  control  design  of  the  flexible  arm  controller,  the  mini-manipulator  controller 
was  designed  using  modern  optimal  control  techniques.  Using  the  model  parameters 
identified  in  Section  3.9,  modal  state  space  models  of  the  position  and  force  dynamics 
were  constructed.  Cost  matrices  and  covariance  matrices  were  chosen,  and  the  corre¬ 
sponding  optimal  regulator  and  estimator  were  then  calculated. 

3.10.1  Position  Control  Design 

3.10.1.1  State  Space  Model 

To  facilitate  control  design,  the  transfer  functions  described  by  Eqs.  (3.89)  and  (3.90) 
were  recast  into  a  state  space  form  given  by 


*r* 

a: 

R'rx^rx  ®rr^r* 

Vrx 

= 

HTxxTX 

*ry 

R'ry^ry  "b  ^ry^ry 

(3.94) 

Vry 

= 

LL  TyXTy. 

Using  a  modal  representation,  one  can  arrive  at  the  following  block  diagonal  form 
for  the  system  dynamics  matrices 


F  = 

rx 


-2Crr,Wr 


(3.95) 


where 


Grx  =  [  0  1  0  1  0  ] 

H rx  =  [  0  0  0  0  1  ] 


[  9rx o  9rro  9rx)  Qri\  Rx 

fu 

Rim 


(3.96) 

(3.97) 


(3.98) 

(3.99) 
(3.100) 
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and 


where 


'  0  1 

**1 

-1 

II 

0  0 

.  ®/p^ry 

~a/P  . 

Gry  =  [O  1  0]T 

Hry  =  [  0  0  1  j 


(3.101) 


(3.102) 

(3.103) 


xry  ~  [  Qryo  Qry0  Rym 

«ry  =  hd 
2/ry  =  Rym  • 


(3.104) 

(3.105) 

(3.106) 


The  quantities  R ^  and  Rym  are  simply  the  filtered  measurements  of  Rx  and  Ry  re- 


spectively.  The  filter  transfer  functions  are 

given  by 

Rxm  j 

«/p  \ 

(3.107) 

Rx  =  ' 

[s  +  clr) 

Rym  j 

a»  ) 

(3.108) 

Ry  '  ' 

la  +  °/r/ 

The  output  matrices  hrx  and  hry  are  defined  by 

Rt  = 

hrxxrx 

(3.109) 

Ry  = 

hTyxry 

(3.110) 

where 

^ri  =  [  rrio 

0  rrr,  0  0  ] 

(3.111) 

hry  —  [  rryo 

0  0  ]  . 

(3.112) 

Again,  to  facilitate  digital  control,  a  discrete  time  model  of  the  system  was  formed. 
The  continuous  time  system  described  by  Eqs.  (3.94)  was  transformed  via  a  zero  order 
hold  into  a  discrete  time  representation  given  by 


Tn+l 

^rxXrxn  4" 

VrXn 

=  ^rr*rrn 

>Vn+l 

^ryxryn  4"  f ry^ry„ 

(3.113) 

yrVn 

—  Hryxry„ • 
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3.10.1.2  Optimal  Regulator  Design 


As  with  the  design  of  the  regulator  for  the  flexible  arm,  one  needs  to  assign  some  cost 
to  state  variables  and  controls.  This  cost  can  then  be  minimized  subject  to  constraint 
equations  —  the  discretized  equations  of  motion  —  to  yield  an  optimal  feedback  gain. 


The  cost  matrices 

R-xxtx ,  R-uurxt  R-zxry ’  and  Ruury  were 

selected  to  yield  acceptable 

closed-loop  roots. 

The  selections  arrived  at  were 

Rxxrx  =  R rx  Rrx 

(3.114) 

Ruurx  =  0.002 

(3.115) 

Rxxry  RryRry 

(3.116) 

Ruury  =  0.004. 

(3.117) 

To  enhance  damping  in  the  system,  some  of  the  costs  associated  with  state  rates,  <j;’ s, 
were  modified.  The  modifications  were 


A**r*(2,2)  <=  Rxxtx{  2, 2) +  5.0 

Rxxrx( 4,4)  <=  tfrrrx(4, 4)  +  10.0  (3.118) 

Rxx ry(2,  2)  •<=  -Rnry(2,  2)  +  5.0. 

Using  these  cost  matrices,  the  optimal  feedback  control  law  is 

Urxn  ~  ^  rxixrxeommand  ~  Xrxn)  (3.119) 

uryn  =  Kry( Xry command.  ~  xryn)  (3.120) 

where  xrxcommand  and  xrycommand  are  the  desired  state  vectors,  and  the  feedback  gains 
are 


Krx  =  [  995.95  54.44  965.50  29.54  0  ]  (3.121) 

Kry  =  [  1340.63  57.42  0  ]  .  (3.122) 

The  actual  control  torques  are  then  calculated  using  the  Jacobian.  Given  uTX  and  ury, 
which  correspond  to  fu  and  fu  respectively,  the  control  torques  are 


Ta 

Tb 


(3.123) 


3.10.1.3  Steady-State  Optimal  Estimator  Design 


As  with  the  optimal  estimator  for  the  flexible  arm,  the  process  noise  and  measurement 
noise  covariances  were  treated  as  design  parameters.  Satisfactory  estimator  performance 
was  achieved  using 


1 

xxrx 

=  r  rT 

*  rx *  rx 

(3.124) 

W* 

=  10~3 

(3.125) 

) 

xxry 

9, 

k. 

II 

(3.126) 

\yr  V 

=  10-3. 

(3.127) 
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Using  the  above  covariance  matrices,  the  steady-state,  optimal  estimator,  feedback  gains 
were  found  to  be 

Lrx  =  [  0.0088  0.2189  -0.0811  0.7031  ]T  (3.128) 

Lry  =  [  0.0076  0.2057  0.8225  ]T.  (3.129) 

Due  to  calculation  time  delays  resulting  from  signal  processing,  a  predictor  estimator 
was  used.  Thus,  the  optimal  estimate  of  the  state  vectors  xTX  and  xry  may  be  found 
via  the  equations 

*r*n+s  =  *rxxrxn  +  rTSUrXn  +  £„(yrx„  -  HrxXrxJ  (3.130) 

Xryn+ 1  —  ^ryxryn  Fry^ryn  "1"  ^ ry{Vryn  ~  H ryxryn)'  (3.131) 


3.10.2  Force  Control  Design 
3.10.2.1  State  Space  Model 

Again,  it  is  convenient  to  recast  Eqs.  (3.91)  and  (3.92)  into  a  modal  state  space  form. 


x/i  =  Ffix/\  +  *2/1  u/i 

yji  =  /T/iX/1 

x/2  =  F }2XJ2  +  Gfi  UJ2 

VJ2  -  Hjix}2- 

In  the  modal  representation,  the  dynamics  matrices  are 


(3.132) 


(3.133) 


where 


and 


G/l 

-  [ 

0  r/1,  0 

lT 

0  r/l0  ] 

(3.134) 

"/! 

=  [ 

0  0  bjn 

0  0  ] 

(3.135) 

*/l  = 

[  <?/i 

7/1  Olm 

1 T 

£flm  £/lm  j 

(3.136) 

«/l  = 

fu 

(3.137) 

VI 1  = 

flm 

(3.138) 

■ 

0  1 

0 

Fn 

= 

0  0 

1 

(3.139) 

L  -bfd4  ~bfd3  ~bfd2 
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where 


CJ 

II 

O 

o 

T 

(3.140) 

hI2  =  [  bJn  0  0  ] 

(3.141) 

f  '  T 

*/2  -  [  £/2m  £/2 m  f/2m 

(3.142) 

U/2  =  fid 

(3.143) 

k 

< 

II 

(3.144) 

lm  are  simply  the  filtered  measurements  of  f\  and  f2  respec- 

tively.  The  filter  transfer  functions  are  given  by 

/lm(^)  fim(a) 

/l(-) 


bfn 


(3.145) 


h(i)  g3  +  b/d2*2  +  bfd3S  +  bfdA 
The  continuous  time  system  described  by  Eqs.  (3.132)  was  transformed  via  a  zero  order 


hold  into  a  discrete  time  representation 

given  by 

+ 

f rru/ln 

*2 

a 

li 

HflXfln 

*/2f»+i  = 

*n*/2n  + 

rf2UI2n 

II 

c 

Hf2xJ2n- 

(3.146) 


%  3.10.2.2  Optimal  Regulator  Design 

Since  the  force  transfer  functions  behave  much  like  a  pure  gain  at  low  frequency,  it 
is  desirable  to  include  integral  control  to  eliminate  steady  state  tracking  error  to  step 
commands,  and  to  provide  good  disturbance  rejection.  To  facilitate  the  addition  of 
integral  control,  the  state  vector  may  be  augmented  with  an  integral  of  the  measured 
forces. 

In  discrete  time,  the  integral  of  the  force  may  be  expressed  by 

e/i„  +  Thmn  =  e/ln  +  rHn  xfln  (3.147) 

e/2„  +  rf2mn  =  ef2n  +  rHf2xf2n  (3.148) 


e/2„+i  = 


where  r  is  the  sample  period  —  0.01  seconds  for  the  sample  frequency  of  100  Hz.  Thus 
the  augmented  state  vector  may  be  written  as 


xf\ „ 
.  c/»» 
a/2» 
.  e/2" 


(3.149) 

(3.150) 
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We  may  now  define  a  new  system  that  contains  the  augmented  state  vectors  x'jx 
and  The  new  discrete  time  dynamics  matrices  are: 

(3.151) 

(3.152) 

(3.153) 

(3.154) 

(3.155) 

(3.156) 

We  will  now  assign  a  cost  to  the  augmented  state  vectors  and  controls  to  arrive  at 
an  optimal  controller  that  minimizes  these  costs.  For  the  lateral  force  controller  which 
regulates  fi,  it  is  desirable  to  assign  a  cost  to  both  the  torsion  mode  and  to  the  integral 
of  the  force.  For  the  longitudinal  force  controller  which  regulates  /2,  it  is  desirable  to 
assign  a  cost  to  the  integral  of  the  force.  Thus,  the  following  cost  matrices  were  arrived 
at: 

0.003  0  0  0  0  0 

0  0  0  0  0  0 

0  0  0  0  0  0 

0  0  0  0  0  0 

0  0  0  0  0  0 

0  0  0  0  0  1 

Ruufi  —  0.015 

0  0  0  0' 

0  0  0  0 
0  0  0  0 
0  0  0  1. 

Rvu/I  =  0.020. 

The  optimal  feedback  control  law  is  then 

U/ln  =  K fl(xflcommand  ~  x/l„)  (3.161) 

u/2n  =  K jicommond  ~  x/2„)  (3.162) 


(3.157) 

(3.158) 

(3.159) 

(3.160) 


where  x and  * /^command  are  the  des»red  state  vectors,  and  the  feedback  gains 
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axe 


Kn  = 


Kn  = 


0.1466 
0.0047 
4.5666  x  106 
1.7084  x  103 
2.8536 
5.7886 


4.6502  x  106 
1.7037  x  103 
2.8530 
5.9288 


(3.163) 


(3.164) 


As  in  position  control,  the  control  torques  are  calculated  using  the  Jacobian.  Given  u/i 
and  «/2,  which  correspond  to  fa  and  fa  respectively,  the  control  torques  are  given  by 
Eqs.  (3.123). 


3.10.2.3  Steady-State  Optimal  Estimator  Design 

Using  process  noise  and  measurement  noise  covariance  matrices  given  by 


Qxxfl  “ 

(3.165) 

Qyvfi  = 

10"3 

(3.166) 

Qxxf2  = 

rnrTn 

(3.167) 

Qwf  2  = 

10~3, 

(3.168) 

satisfactory  estimator  performance  was  achieved.  The  corresponding  steady-state,  op¬ 
timal  estimator,  feedback  gains  were 

0.3215 
12.4454 
7.5940  x  10~9 
-3.7997  x  10-7 
1.6615  x  10"4 
0 

4.6368  x  10"9 
-6.9493  X  10-7 
1.7783  x  lO’4 
0 

Again,  due  to  computation  time  delays  resulting  from  signal  processing,  a  predictor 
estimator  was  implemented  to  estimate  the  force  states.  Thus,  the  optimal  estimate  of 
the  state  vectors  x ^  and  x may  be  found  via  the  equations 

*/i»+i  =  */ix/i„  +  ri\uJU  +  LfiiVfin  ~  (3.171) 

*/2„+,  =  ^/2*/2«  +■  FjlUjIn  +  £/2(y/2n  ~  Hj2xJ2n)'  (3.172) 


(3.169) 


(3.170) 
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Figure  3.16:  Position  Control  Step  Response  for  the  Mini-Manipulator 


3.11  End-Point  Control  of  the  Mini-Manipulator 

Using  the  optimal  regulators  and  estimators  designed  in  Sections  3.7  and  3.10,  end¬ 
point  position  and  force  control  of  the  flexible  arm  and  mini-manipulator  system  is 
now  possible.  The  position  controller  for  the  flexible  arm  and  the  position  and  force 
controller  for  the  mini-manipulator  were  designed  separately.  This  was  made  possible 
by  the  fact  that  the  flexible  arm  and  the  mini-manipulator  are  bandwidth  separated  — 
the  mini-manipulator  is  about  six  times  as  fast  as  the  flexible  arm. 

Running  the  controller  at  a  sample  rate  of  100  Hz,  the  mini-manipulator  was  given 
a  step  command  in  end-point  position.  Figure  3.16  shows  the  time  history  of  the  end¬ 
point  of  the  mini-manipulator.  The  roughly  5  cm  step  takes  0.11  seconds  to  complete. 
In  comparison,  the  flexible  arm  required  about  0.65  seconds  to  complete  a  5  cm  slew. 

Next,  the  mini-manipulator  was  given  a  step  command  in  end- point  force.  In 
Fig.  3.17  we  see  a  time  history  of  the  force  level  during  this  step  command.  It  takes 
about  0.06  seconds  to  complete  a  transition  in  force  level  from  21  grams  force  to  35 
grams  force.  In  comparison,  the  time  required  to  achieve  a  step  change  in  force  level 
using  the  earlier  flexible  arm  system,  which  did  not  have  a  mini-manipulator,  was  about 
0.7  seconds. 
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Figure  3.17:  Force  Control  Step  Response  for  the  Mini-Manipulator 

3.12  Surface  Following 

As  humans,  we  can  appreciate  the  value  of  being  able  to  follow  the  contours  of  surfaces 
using  touch.  It  is  an  ability  that  is  useful  in  tasks  such  as  cleaning  windows,  mopping 
floors,  and  sanding  wood.  A  robot  with  this  ability  would  hence  be  very  valuable.  Since 
the  flexible  arm  and  mini-manipulator  system  possess  a  sense  of  touch,  namely  the  force 
sensing  beam,  it  is  possible  to  demonstrate  a  surface  following  ability. 

When  the  mini-manipulator  end-point  comes  into  contact  with  an  object,  it  senses 
a  force.  Because  the  force  sensing  beam  is  equipped  with  a  contact  roller,  the  forces 
sensed  are  ideally  only  normal  forces.  Thus,  given  the  direction  of  the  contact  force, 
one  knows  that  the  surface  normal  lies  in  the  opposite  direction.  The  surface  tangent 
is  then  orthogonal  to  this.  Thus,  one  knows  the  local  direction  of  the  surface,  and  it  is 
possible  to  move  in  this  direction  while  applying  forces  in  the  surface  normal  direction. 

The  surface  following  algorithm  that  was  implemented  is  as  follows.  At  time  to, 
determine  the  surface  normal  and  tangent  directions.  Next,  command  a  position  that 
lies  a  distance  k  in  the  tangent  direction.  Also,  command  a  force  in  the  direction  opposite 
to  the  surface  normal.  Now,  at  a  time  fi  that  is  one  sample  period  after  to,  determine 
the  new  surface  normal  and  tangent  directions.  Project  the  old  commanded  position 
onto  a  line  that  is  parallel  to  the  new  surface  tangent  and  passes  through  the  current 
contact  point.  To  this  projected  position,  step  a  distance  k  in  the  tangent  direction  and 
define  this  as  the  new  commanded  position.  Again,  command  a  force  in  the  direction 
opposite  to  the  current  surface  normal.  Repeat  this  every  sample  period. 
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One  may  also  state  this  algorithm  in  a  more  mathematical  form.  If  the  current 
applied  force  vector  is 

f  =  fxnl  +  fyn  2>  (3.173) 

then  the  current  surface  normal  vector  is 


i 


—  !ll»l  4“ 

„  fy  _  /* 

’  T7T  *  “  17t 


(3.174) 


Let  (Pi,pv)  be  the  Cartesian  coordinates  of  the  current  end-point  position,  and  let 
(PxdiPvci)  be  the  Cartesian  coordinates  of  the  last  commanded  position.  Then  the 
projection  of  the  last  commanded  position  on  a  line  passing  through  (px,pv)  and  parallel 
to  t  is 

l  —  ( Pxcl  ~  Px)tz  4"  (Pyel  ~  Py)ty  (3.175) 

Finally  Cartesian  coordinates  of  the  next  commanded  position,  (pXC2»P»c2)>  are  given  by 


Pxc2  =  Px  4-  (/  4-  k)tx  (3.176) 

Pyc 2  =  Pv  4"  (1  +  &)fy*  (3.177) 

where  fc  is  a  constant. 

This  algorithm  involves  position  control  in  one  direction,  and  force  control  in  an 
orthogonal  direction.  One  will  recall  that  the  position  controller  generates  a  control 
effort  uTX  corresponding  to  fu,  and  ury  corresponding  to  fu-  Let  fidpo,  and  fu^,  be 
the  control  efforts  determined  by  the  position  controller.  The  force  controller  generates 
a  control  effort  uji  corresponding  to  fu,  and  u/2  corresponding  to  fu-  Let  fut„  and 
hdfor  be  the  control  efforts  determined  by  the  force  controller.  The  combined  position 
and  force  control  effort  is  then 


fu, 

fu 


combined 


combitfd 


One  then  uses  the  relation 


fu,,,  4-  f\d}„ 

fUpct  4~  f 2d  far  • 


Ta 

Tb 


JT 


fu 

fu 


combined 
combined  m 


(3.178) 

(3.179) 


(3.180) 


to  determine  the  joint  torques. 

Using  this  algorithm,  the  flexible  arm  was  commanded  to  follow  the  contours  of 
a  target  at  a  speed  of  4  cm/sec.  Figure  3.18  shows  an  experimental  plot  describing 
this  maneuver.  The  solid  line  describes  the  path  made  by  the  end-point  of  the  mini- 
manipulator,  and  essentially  outlines  the  shape  of  the  target  surface.  The  many  short, 
straight  lines  emanating  from  the  target  surface  describe  what  the  force  controller  es¬ 
timated  the  local  surface  normal  to  be.  These  estimates  are  shown  every  0.2  seconds. 
Based  on  measurements  of  link  angles  and  LED  positions,  the  configuration  of  the  flex¬ 
ible  arm  and  mini-manipulator  is  shown  every  3.0  seconds. 
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3.13  Conclusion 

End-point  position  and  force  control  of  a  flexible  arm  with  a  two  degree-of- freedom 
mini-manipulator  has  been  demonstrated.  The  end-point  position  of  the  flexible  arm 
can  be  controlled  at  a  bandwidth  of  1.5  Hz  —  this  compares  quite  favorably  with  the 
first  cantilever  mode  of  the  flexible  arm  which  is  0.7  Hz. 

The  torsional  interactions  of  the  mini-manipulator  and  the  flexible  arm  were  mod¬ 
eled.  From  this,  it  was  determined  that  the  original  system  could  be  modified  to  greatly 
simplify  position  control  by  providing  a  favorable  pole-zero  configuration.  At  the  same 
time,  it  was  shown  that  there  was  no  convenient  means  of  achieving  this  favorable 
pole-zero  configuration  for  force  control. 

A  generic  means  of  mapping  a  non-linear  system  into  an  approximately  linear  one 
was  demonstrated.  This  approach  is  applicable  to  any  system  with  a  square  Jacobian, 
and  linkages  that  are  light  compared  to  the  end-point  mass.  In  this  experiment,  a 
non-linear  mini-manipulator  was  mapped  into  an  approximately  linear  system.  This 
linearization  facilitated  the  use  of  standard  techniques  for  identifying  and  controlling 
linear  systems. 

Using  optimal  control  techniques,  an  end-point  position  and  force  controller  were 
developed  for  the  mini-manipulator.  These  controllers  took  advantage  of  the  fact  that 
the  flexible  arm  and  the  mini-manipulator  could  be  bandwidth  separated.  This  made  it 
possible  to  design  controllers  for  the  mini-manipulator  and  the  flexible  arm  separately. 
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The  resulting  bandwidth  for  position  and  force  control  at  the  end-point  of  the  mini- 
manipulator  was  about  10  Hz  —  roughly  ten  times  better  than  for  a  flexible  arm  without 
a  mini-manipulator. 

Finally,  the  ability  to  follow  the  contours  of  a  surface,  without  a  priori  knowl¬ 
edge  of  its  shape,  was  demonstrated.  This  involved  having  the  end-point  of  the  mini¬ 
manipulator  “feel”  its  way  around  an  object.  It  also  demonstrated  the  ability  to  nego¬ 
tiate  sharp,  right  angle  changes  in  surface  orientation.  The  surface  following  algorithm 
is  insensitive  to  slow  motions  of  the  target  object. 

3.14  Status 

The  work  described  in  this  chapter  is  in  a  state  of  near  completion.  The  original  goals 
°f  demonstrating  position  and  force  control,  as  well  as  surface  following,  have  been 
.complished.  This  work  is  currently  being  written  up  as  a  PhD  thesis. 


Chapter  4 

Force  Control  of  a  Two-Link 
Arm  With  Flexible  Drive  Train 


Brian  Andersen 


Introduction 

A  crucial  capability  for  two-arm  cooperation  will  be  exquisite  force  control  at  each  arm 
tip.  The  supporting  research  effort  described  in  this  chapter  is  aimed  at  advancing  that 
capability  in  the  context  of  a  single  two-link  arm  having  a  very  flexible  drive  train. 

This  section  of  the  final  report  describes  work  that  has  been  accomplished  to  date 
on  the  two-link  arm  with  flexible  tendon  drive  and  mini-manipulator.  The  objective 
of  this  research  is  to  allow  the  arm  to  slew  into  contact  with  a  target,  make  a  smooth 
touchdown,  and  maintain  a  constant  force  on  the  target,  using  the  mini-manipulator 
for  augmentation  of  precision  control.  We  believe  the  mini-manipulator  is  going  to  play 
a  key  role  in  successful  high-performance  force  control.  In  this  chapter  we  describe 
its  use  on  a  two-link  arm.  Chapter  4  describes  additional  fundamental  force-control 
experiments  with  the  two-degree-of-freedom  mini-manipulator  mounted  on  a  single,  very 
flexible  beam. 

During  the  time  covered  by  this  report  we  have  demonstrated  initial  position  and 
force  control  with  the  mini-manipulator  on  a  fixed  base.  This  work  will  form  the  basis 
for  the  control  algorithms  when  the  mini-manipulator  is  added  to  the  two-link  arm. 

This  is  a  facility  which  was  completed  three  years  ago,  and  with  which  we  have 
already  demonstrated  precise,  rapid  control  of  very  large  pick-and-place  tasks  with  heavy 
targets  of  varying  weight.  A  schematic  representation  of  the  two-link-arm  apparatus  is 
shown  in  Fig.  4.1,  in  which  the  mini-manipulator  is  not  shown  to  scale.  The  main  arm 
actually  has  approximately  six  ?!  res  the  reach  of  the  mini-manipulator. 

The  rationale  for  using  a  i.-ui-manipulator  at  the  tip  of  the  two-link  arm  is  that, 
for  most  robotic  tasks,  a  great  deal  of  the  manipulation  is  performed  in  a  few  localized 
workspaces.  A  mini-manipulator  can  be  used  to  perform  these  tasks  in  the  localized 
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Figure  4.1:  The  Two-Link  Arm  with  Mini-Manipulator 

workspace  at  a  much  higher  bandwidth  than  is  achievable  by  the  main  arm.  The  main 
arm  is  used  to  move  the  mini-manipulator  between  the  localized  workspaces. 

The  status  of  this  project  is  that  the  experimental  hardware  has  been  built  and 
is  functional.  The  computer  hardware  and  interface  electronics  have  also  been  built 
and  are  functional.  Initial  experiments  with  the  mini-manipulator  on  a  fixed  base  have 
demonstrated  position  and  force  control.  Using  the  same  experimental  set-up,  following 
a  surface  while  maintaining  a  desired  force  has  also  been  demonstrated.  This  work  can 
be  extended  when  the  mini-manipulator  is  added  to  the  two-link  arm. 

This  chapter  of  the  report  describes  the  various  demonstrations  that  will  be  per¬ 
formed  to  test  the  control  methods  developed,  the  design  of  the  experimental  hardware, 
the  various  position  and  force  control  algorithms  that  have  been  developed,  results  from 
their  experimental  implementation,  and  future  plans  for  this  project. 


4.1  Objective  of  this  Project 

The  capability  we  wish  to  develop  here  is  for  very-high-bandwidth,  precise  control  of 
the  force  at  the  tip  of  a  two-link  manipulator  having  a  very  flexible  drive  train.  We 
plan  to  demonstrate  this  capability  by  (1)  performing  rapid  slew  and  touch  with  a  fixed 
target  while  having  no  overshoot  in  the  force,  (2)  controlling  the  arm  so  that  its  tip 
moves  along  a  wavy  surface  while  maintaining  a  constant  force  on  the  surface,  and  (3) 


Figure  4.2:  Demonstration  of  Force  Control  Capabilities 

slewing  into  contact  with  a  moving  target  and  maintaining  a  constant  force  on  it  while 
it  continues  to  move. 

The  initial  experiments  will  be  performed  with  some  massive  object  for  a  target  that 
will  not  move  when  the  arm  tip  exerts  a  force  against  it.  The  important  elements  to 
demonstrate  are  that  the  arm  is  controlled  accurately  both  in  and  out  of  contact  with 
the  environment,  and  that  there  is  quick  switching  between  modes  of  control.  This  will 
be  preceded  by  performing  the  same  experiment  with  the  mini-manipulator  on  a  fixed 
base. 

The  next  demonstration,  shown  in  Fig.  4.2,  requires  a  massive  target  which  will  have 
surface  waves  of  the  order  of  magnitude  of  the  range  of  the  mini-manipulator.  The  arm 
will  be  controlled  to  maintain  a  constant  force  while  following  the  surface.  The  main 
arm  will  be  controlled  to  track  primarily  along  a  straight  line  along  the  target  while  the 
mini-manipulator  follows  the  waves  in  the  target  surface. 

This  demonstration  will  also  be  performed  with  the  mini- manipulator  on  a  fixed 
base.  In  some  ways,  performing  this  task  on  a  fixed  base  is  more  difficult.  When  the 
mini-manipulator  is  attached  to  the  main  arm,  the  tip  of  the  main  arm  can  be  moved  so 
that  the  mini-manipulator  is  in  the  center  of  its  workspace.  When  the  mini-manipulator 
is  on  a  fixed  base,  however,  it  can  only  apply  the  desired  force  wherever  the  target  is  in 
its  workspace. 

The  final  demonstration,  shown  in  Fig.  4.3,  will  address  controlling  force  applied 
to  a  moving  target.  We  plan  to  use  as  a  target  a  model  railroad  train  similar  to  one 
that  has  been  developed  for  another  experiment  and  which  should  require  little  if  any 
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Figure  4.3:  Demonstration  of  Force  Control  Capabilities 

modification.  This  will  demonstrate  the  ability  to  follow  a  moving  surface  in  a  large- 
disturbance  environment. 


4.2  Experimental  Apparatus 

4.2.1  Two-Link  Arm 

The  experimental  two-link  manipulator  is  a  SCARA  manipulator  operating  in  the  hori¬ 
zontal  plane  with  two  revolute  joints  and  vertical  plunge  actuator  at  the  tip.  A  drawing 
of  the  manipulator  is  shown  in  Fig.  4.4  with  the  base,  supports,  and  mini-manipulator 
removed  to  show  the  essential  features.  Both  of  the  links  of  the  manipulator  are  ap¬ 
proximately  0.5m  in  length.  The  reach  of  the  manipulator  ranges  from  a  minimum  of 
about  0.5m  to  a  maximum  of  about  1.0m.  The  vertical  plunge  actuator  gives  a  range 
of  motion  of  about  15.0cm. 

The  links  of  the  manipulator  are  driven  by  two  identical  DC  electric  motors  with 
maximum  torque  capability  of  11.2iV  -  m.  The  upper  link  is  connected  to  the  shoulder 
motor  via  a  reduction  stage  and  a  cable  drive  with  four  springs  mounted  in-link  to 
provide  for  exaggerated  flexibility.  Similarly,  the  forearm  is  connected  to  the  elbow 
motor  via  an  idler  pulley  and  a  cable  drive  with  two  springs  mounted  in-line.  The 
shoulder  drive  train  provides  a  5.91:1  gear  reduction,  and  the  elbow  drive  train  provides 
a  2.44:1  gear  reduction. 

The  motors,  both  joints  of  the  manipulator  and  the  vertical  plunge  actuator  are 


4.2.  EXPERIMENTAL  APPARATUS 


Figure  4.4:  Drawing  of  the  Experimental  Two-Link  Manipulator 
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Figure  4.5:  The  Mini-Manipulator 

equipped  with  rotary  encoders  to  provide  angular  position  information.  These  encoder 
signals  are  processed  electronically,  using  a  first  difference  at  low  rates  or  a  period 
count  at  high  rates,  to  give  angular  velocity  information.  In  addition,  each  motor  has  a 
tachometer  to  provide  angular  velocity  information.  Finally,  there  is  an  optical  sensor 
that  senses  center  of  brightness.  This  observes  the  triad  of  infrared  LEDs  mounted 
on  the  top  of  the  vertical  plunge  actuator  to  determine  the  end-point  position  of  the 
manipulator. 

4.2.2  Mini-Manipulator 

The  mini-manipulator  is  a  small  five-link,  closed-kinematic-chain  manipulator  which 
operates  in  a  horizontal  plane.  It  is  shown  schematically  in  Fig.  4.5.  The  connection 
between  the  two-link  manipulator  and  the  mini-manipulator  is  shown  in  greater  detail 
in  Fig.  4.6.  The  base  link,  which  is  rigidly  attached  to  the  vertical  plunge  actuator 
of  the  two-link  manipulator,  is  approximately  5.1cm  in  length.  The  two  inner  links  of 
the  mini-manipulator  are  approximately  7.6 cm  in  length  and  the  two  outer  links  are 
approximately  10.2cm  in  length.  The  inner  and  outer  links  are  hollow  tubes  for  light 
weight  and  fast  speed.  At  the  end  of  the  outer  links  where  they  are  connected  through 
a  revolute  joint,  there  is  a  vertical  beam  with  a  square  cross  section.  At  the  bottom 
end  of  this  beam  is  a  bearing  enclosed  by  a  hard  rubber  ring.  It  is  the  rubber  ring  that 
makes  contact  with  the  environment. 

The  two  inner  links  of  the  mini-manipulator  are  driven  relative  to  the  base  link  by 
two  small  DC  electric  motors.  These  motors  have  a  peak  torque  of  0.35iV  -  m. 

Mounted  on  top  of  the  mini-manipulators  are  rotary  encoders  to  provide  angular 
position  information.  These  encoder  signals  are  processed  using  the  same  electronics  as 
the  two-link  arm  encoders  to  give  angular  velocity  information. 

The  vertical  beam  at  the  tip  of  the  mini-manipulator  is  equipped  with  two  pairs  of 
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Figure  4.6:  Connection  of  the  Two-Link  Arm  with  Mini-Manipulator 

strain  gages  near  the  top  of  the  beam.  One  gage  is  mounted  on  each  face  of  the  beam. 
Each  pair  of  gages  on  opposite  faces  of  the  beam  is  connected  in  a  bridge  configuration 
with  two  resistors  on  a  strain-gage  amplifier  board  to  produce  a  signal  proportional 
to  the  force  applied  in  one  direction  with  only  a  small  amount  of  noise.  These  gages 
and  electronics  are  used  to  produce  an  estimate  of  the  force  applied  at  the  tip  of  the 
mini-manipulator  in  two  perpendicular  directions  in  the  horizontal  plane. 

4.2.3  Control  Computer  and  Interface  Electronics 

The  real-time  control  of  the  mechanical  system  is  handled  by  a  Motorola  68020  single¬ 
board  processor  with  a  68881  floating  point  coprocessor.  Both  operate  at  16.67AT.ffz. 
This  board  is  connected  via  a  bus  repeater  to  a  Sun  3/160  workstation  using  the  VME 
Bus.  Using  this  connection,  software  can  be  developed  and  debugged  in  the  flexible 
development  environment  of  the  Sun  workstation  and  then  down-loaded  to  the  68020 
processor.  Using  software  developed  in  this  laboratory,  the  68020  processor  controls 
the  experimental  hardware  while  sending  data  through  the  VME  Bus  back  to  the  Sun 
workstation  for  display  and  processing. 

Control  signals  are  output  from  the  computer  using  the  VMIVME-4116  8-channel 
16-bit  Digital-to-Analog  Converter.  Analog  signals,  such  as  the  force  sensor  and  the 
motor  tachometers,  are  sampled  by  the  XVME-566  Analog-to- Digital  Converter  that 
provides  16  differential  input  channels  with  12-bit  resolution.  Finally,  the  MVME340 
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Parallel  Interface/Timer  Module  drives  the  encoder  interface  electronics  built  in  the 
laboratory  and  provides  for  output  of  discrete  signals  such  as  a  motor  heartbeat.  All 
three  of  these  boards  are  connected  to  the  68020  processor  via  a  VME  Bus  backplane. 


4.3  Analysis  and  Controller  Design 

4.3.1  Equations  of  Motion 

The  equations  of  motion  of  the  two-link  arm  with  mini-manipulator  and  the  mini¬ 
manipulator  alone,  both  in  and  out  of  contact  with  its  environment,  have  been  deter¬ 
mined.  These  equations  will  be  used  for  simulation  and  for  comparison  with  the  actual 
system. 

The  equations  were  derived  using  Kane’s  dynamics.  It  includes  intermediate  pul¬ 
leys  between  the  motors  and  the  joints  of  the  main  arm.  Friction  is  included  in  the 
motors,  springs,  and  joints  of  the  main  arm.  It  is  not  included  in  the  joints  of  the  mini- 
manipulator  since  that  is  considered  to  be  small  compared  to  the  friction  in  the  rest  of 
the  system.  This  could  be  included  later  without  much  difficulty.  The  environment  is 
modelled  as  a  spring  and  a  linear  damper  attached  at  one  end  to  a  point  of  contact  on 
the  target  and  to  the  tip  of  the  arm  on  the  other  end.  This  models  any  elasticity  in  the 
target,  force  sensor,  or  both.  A  large  spring  constant  for  the  environment  can  be  used 
if  it  is  desired  to  model  the  target  surface  as  rigid. 

The  system  as  modelled  has  eight  rigid  bodies  (two  main  arm  motors,  two  main  arm 
links,  and  only  four  mini-manipulator  links  since  the  base  link  is  rigidly  attached  to  the 
fore  arm  link)  and  four  constraint  equations  (the  matching  of  position  and  velocity  of 
the  ends  of  the  two  outer  links  of  the  mini-manipulator),  for  a  total  of  twelve  degrees 
of  freedom  for  the  system.  Due  to  the  complexity  of  these  twelve  equations  of  motion, 
they  are  not  presented  here. 

The  equations  of  motion  for  both  the  mini-manipulator  alone  and  the  mini-manipulator 
on  the  end  of  the  two-link  arm  have  been  incorporated  into  a  simulation.  The  simula- 
tion  is  written  as  subroutines  tb  are  linked  into  MatrixX/System  Build,  a  non-linear 
system  simulation  package. 

4.3.2  Control  Methods 

The  combination  of  the  main  two-link  arm  and  the  mini-manipulator  makes  an  intricate 
dynamic  system  with  very  complicated  equations  of  motion.  Rather  than  try  to  develop 
a  controller  design  that  can  handle  the  complicated  dynamics  of  the  whole  system, 
we  take  advantage  of  the  fundamental  dynamic  behavior  of  the  system.  The  mini¬ 
manipulator  is  designed  to  be  smaller  and  quicker  than  the  main  two-link  arm,  so  its 
control  bandwidth  will  be  much  larger.  We  can  take  advantage  of  this  spectral  separation 
of  the  two  systems  to  design  controllers  for  the  two  systems  independently.  The  main 
arm  will  have  a  slower  controller  designed  without  considering  the  dynamics  of  the  mini¬ 
manipulator.  This  is  a  good  approximation  since  the  mini-manipulator  dynamics  have 


4.3.  ANALYSIS  AND  CONTROLLER  DESIGN 


83 


a  small  effect  on  the  dynamics  of  the  main  arm. 

The  mini-manipulator  will  have  a  control  algorithm  designed  independently  of  the 
dynamics  of  the  main  arm.  However,  because  the  dynamics  of  the  main  arm  are  strongly 
coupled  with  the  dynamics  of  the  mini-manipulator,  the  motion  of  the  main  arm  must 
be  taken  into  effect  in  some  way.  This  probably  can  be  accomplished  by  simply  using  the 
kinematic  quantities  of  the  tip  of  the  main  arm,  such  as  position,  velocity,  and  possibly 
acceleration.  The  exact  method  has  yet  to  be  determined. 

4.3.2. 1  Main  Arm 

Control  of  the  main  arm  will  draw  on  the  work  of  Michael  Hollars  from  this  laboratory. 
He  demonstrated  high-bandwidth  position  control  for  the  tip  of  the  main  arm  carrying  a 
variety  of  payload  masses,  as  described  in  [5].  His  control  algorithm  involved  designing 
a  Linear  Quadratic  Regulator  (LQR)  for  the  system  linearized  about  an  elbow  angle 
of  35°.  Attempts  to  use  a  Kalman  Filter  for  the  estimator  failed,  however,  due  to  the 
large  non-linearities  in  the  system.  Instead  of  a  linear  estimator,  a  method  called  the 
Constant  Gain  Extended  Kalman  Filter  (CGEKF)  was  used.  This  method  uses  the 
same  linear  measurement  update  as  the  usual  Kalman  Filter.  Instead  of  using  a  linear 
state  update,  though,  it  integrates  the  full  equations  of  motion  of  the  system  to  arrive 
at  the  new  state  estimate.  This  method  has  proved  to  work  very  well  for  the  two-link 
arm  with  no  mini-manipulator  while  not  in  contact  with  its  environment. 

Specifically,  the  CGEKF  was  implemented  in  three  equations:  a  measurement  up- 


date: 

x(k)  =  x(fc)  +  Ld(y(k)  -  Cdx{k)) 

(4.1) 

a  time  update: 

rt(k+ 1) 

i(*  +  l)-  /  f(x(k),u(k))dt 

(4.2) 

and  finally  the  control  equation: 

u(k  +  1)  =  Kd(xc(k  +  1)  -  x(fc  +  1)) 

(4-3) 

where  x(k)  is  the  estimated  value  of  x  at  the  current  time,  x(k+ 1)  is  the  predicted  value 
at  the  next  time,  Li  is  the  estimator  gain  matrix,  y(k)  is  the  current  measurement,  Cd 
is  the  matrix  that  converts  states  to  measured  values,  u(k)  is  the  control  torque,  Ki  is 
the  controller  gain  matrix,  and  x  =  f(x(k),u(k))  are  the  nonlinear  equations  of  motion 
of  the  system. 

4.3.2. 2  Mini-Manipulator 

To  date,  there  have  been  three  control  algorithms  tested  experimentally  using  the  mini¬ 
manipulator  on  a  fixed  base.  The  first  two  of  these  are  methods  for  controlling  the 
position  of  the  tip,  and  the  third  controls  the  force  applied  by  the  tip. 

The  first  control  algorithm  tested  was  a  simple  Proportional-Derivative  (PD)  control 
loop,  using  the  motor  angles.  This  scheme  indirectly  controls  tip  position  by  using  the 
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inverse  kinematics  of  the  mini-manipulator  to  determine  the  angles  that  give  the  desired 
tip  position.  It  does  not,  however,  address  the  motion  of  the  tip  through  the  workspace. 

The  second  control  algorithm  tested  was  an  impedance  controller.  This  control 
algorithm  was  originally  described  by  Hogan  in  [3].  This  control  scheme  takes  into 
account  the  kinematics  of  the  mini- manipulator  so  that  the  tip  is  controlled  directly  in 
Cartesian  space.  However,  it  does  not  consider  the  dynamics  of  the  mini-manipulator, 
so  it  does  not  compensate  for  dynamic  effects  such  as  centrifugal  acceleration.  It  can 
be  considered  in  a  somewhat  simplistic  view  as  a  PD  controller  of  the  tip  position  and 
velocity. 

An  impedance  controller  establishes  a  fixed  relation  between  the  position  of  the  tip 
and  the  force  on  the  tip.  In  this  case,  the  relationship  is 

F  =  Kp(x  -  xdet)  +  Kv(x  -  xdet)  (4.4) 

where  F  and  x  are  the  force  on  and  the  position  of  the  tip,  respectively,  xdei  and  xdea 
are  the  desired  position  and  velocity  of  the  tip,  and  Kp  and  Kv  are  gains  that  can  be 
adjusted  to  change  the  impedance  of  the  arm.  Using  the  relation 

r  =  JtF  (4.5) 

where  J  is  the  Jacobian  of  the  mini-manipulator,  this  relation  becomes 

T  =  JtKp(x  -  xde$)  +  JtKv(x  -  xdet)  (4.6) 

where  T  is  the  torque  of  the  mini- manipulator  motors. 

The  non-dynamic  impedance  controller  just  described  has  the  desirable  property  of 
regulating  the  interaction  between  the  error  in  tip  position  and  the  force  applied  to  the 
environment,  at  least  in  a  static  sense.  It  can  be  said  to  be  a  method  of  regulating 
the  force  applied  to  the  environment.  Unfortunately,  the  input  to  this  controller  is  a 
desired  position,  not  a  desired  force.  It  is  possible  to  determine  a  desired  position  from 
a  desired  force,  but  only  if  the  exact  position  of  the  edge  of  the  target  and  the  combined 
compliance  of  the  target  and  end-effector  are  known.  In  practice,  neither  of  these  will 
be  known  exactly.  Moreover,  the  properties  of  the  target  must  be  known,  so  all  objects 
would  have  to  be  categorized  in  advance,  thus  limiting  the  generality  of  the  algorithm. 

To  overcome  these  limitations,  feedback  can  be  used  to  set  the  desired  position 
instead  of  setting  it  a  priori.  For  example,  it  can  be  modified  using  the  difference 
between  the  desired  and  actual  force.  This  is  the  basis  of  the  third  control  algorithm 
that  has  been  experimentally  tested.  The  same  impedance  controller  operates  at  all 
times,  whether  in  or  out  of  contact  with  the  environment.  When  contact  has  been 
made,  however,  a  control  loop  is  used  to  regulate  the  desired  position.  In  this  method, 
the  force  error,  integral  force  error,  and  error  in  velocity  normal  to  the  perceived  surface 
of  the  target  are  combined  to  determine  an  incremental  change  in  the  desired  position. 
The  desired  position  thus  determined  is  then  used  by  the  impedance  controller. 
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The  distance  within  the  surface  to  place  the  desired  position  is  determined  by  the 
equation 

Step  =  Kp(F  -  Fjet)  +  KvN„malx Normal  +  Ki  J (F  -  Fdet)dt  (4.7) 

where  Kf,  KvN„mal,  and  K\  are  constant  gains,  F  and  are  the  actual  and  desired 

force  applied  by  the  arm  respectively,  XNormai  is  the  velocity  normal  to  the  surface  of 
the  target,  and  Step  is  the  distance  inside  the  surface  to  command  the  desired  position. 
Using  this  value,  the  desired  positions  for  the  impedance  controller  can  be  computed 
using  the  equations 

Xde»  =  xtip  -  Step  cos  Bp  (4.8) 

l Ides  =  Vtip  ~  Step  sin  Bp  (4.9) 

where  Xdet  and  Vde»  are  the  Cartesian  coordinates  of  the  desired  position  of  the  tip,  xtip 
and  ytip  are  the  actual  position  of  the  tip,  and  Bp  is  the  angle  at  which  force  is  applied 
by  the  target. 

With  this  scheme,  the  addition  of  sliding  along  the  surface  is  not  very  difficult.  The 
desired  velocity  along  the  surface  can  be  set  to  the  desired  sweep  speed.  The  desired 
position  is  set  to  the  desired  position  calculated  by  the  force  loop  in  Eqns.  4.8  and 
4.9  plus  a  step  along  the  surface  equal  to  the  sweep  speed  multiplied  by  the  sample 
period.  In  this  way,  the  calculation  of  the  desired  position  along  and  into  the  surface 
are  independent  of  one  another. 

The  addition  of  sliding  along  the  surface  does  not  change  Eqn.  4.7.  Eqns.  4.8  and 
4.9  become 


x<u»  =  xtip  -  Step  cos  Bp  -  VsweepTs  s'mBp 

(4.10) 

Vde*  =  Vtip  -  Step  sin  Bp  +  VSweepTs  cos  Bp 

(4.11) 

with  the  additional  equations 

Xdet  ~  ~  VSwcep  sin  Bp 

(4.12) 

Vde*  =  Vsweep  COS  Bp 

(4.13) 

where  VsWeep  is  the  desired  speed  at  which  to  sweep  along  the  surface,  T$  is  the  sample 
period,  and  idea  and  ydet  are  the  Cartesian  coordinates  of  the  desired  velocity  tor  the 
impedance  controller. 


4.4  Experimental  Results 

This  section  will  examine  and  discuss  the  experimental  results  obtained  to  date.  All 
results  are  for  the  mini-manipulator  on  a  fixed  base.  All  three  control  methods  discussed 
in  Sect.  4.3.2.2  are  demonstrated  and  discussed. 
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Figure  4.7:  Step  Command  in  Angie  for  PD  Angle  Controller 
4.4.1  Position  Control 

The  PD  angle  controller  was  given  a  step  command  such  that  the  mini-manipulator  tip 
moved  parallel  to  the  X  axis  from  a  Y  coordinate  of  10  cm  to  a  Y  coordinate  of  0  cir 
The  angle  of  motor  1  versus  time  is  shown  in  Fig.  4.7.  The  motor  1  angle  follows  a 
well-damped  trajectory  to  the  new  desired  angle.  This  is  expected  because  the  motor 
angles  are  being  controlled. 

The  motor  angles  of  the  mini-manipulator  are  not  the  important  quantities,  however. 
It  is  the  position  of  the  tip  of  the  mini-manipulator  that  must  be  controlled.  The 
response  of  the  tip  of  the  mini-manipulator  in  Cartesian  space  is  shown  in  Fig.  4.8. 
The  trajectory  followed  by  the  tip  has  a  large  curve  in  it  and  is  not  the  straight  path 
that  is  desired.  This  shows  graphically  the  result  of  controlling  the  motor  angles  and 
not  the  tip  position. 

The  impedance  controller,  on  the  other  hand,  is  controlling  position  of  the  mini¬ 
manipulator  tip  directly.  The  response  of  the  mini-manipulator  tip  to  the  same  step 
command  described  for  the  PD  controller  is  shown  in  Fig.  4.9.  The  tip  position  follows 
a  nicely  damped  trajectory,  which  is  the  desired  behavior. 

A  comparison  of  the  manipulator  tip  trajectories  for  the  two  controllers  is  shown 
in  Fig.  4.101.  This  shows  clearly  the  superior  behavior  of  the  impedance  controller 
in  following  a  straight-line  trajectory  at  the  mini-manipulator  tip.  Other  than  the 
deviation  at  the  end  of  the  trajectory,  which  might  be  due  to  the  friction  in  the  system, 

‘These  trajectories  do  not  start  and  end  at  exactly  the  same  place  because  these  points  were  specified 
to  one  in  terms  of  angles  and  the  other  in  terms  of  cartesian  tip  coordinates.  In  addition,  friction  causes 
positioning  errors. 


Y  Tip  Position  (mm)  (S'  Y  Tip  Position  (mm) 
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Cartesian  Trajectory  for  PD  Angle  Controller 


Figure  4.9:  Step  Command  in  Y  Coordinate  for  Impedance  Controller 
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Figure  4.10:  Trajectory  Comparison  for  Two  Position  Controllers 

the  impedance  controller  takes  the  mini-manipulator  tip  very  close  to  a  straight  line. 
The  time  tics  on  the  plot  are  spaced  evenly  at  approximately  40  msec  intervals  along  the 
trajectory.  Note  that  not  only  is  path  closer  to  a  straight  line,  the  impedance  controller 
takes  about  half  the  time  to  arrive  in  the  vicinity  of  the  desired  point. 

However,  the  impedance  controller  implemented  is  strictly  non-dynamic  control.  It 
doesn’t  take  into  account  any  of  the  acceleration  terms  in  the  equations  of  motion.  Its 
ability  to  follow  a  straight-line  trajectory  should  therefore  get  worse  over  larger  slews 
when  the  manipulator  is  moving  faster  and  these  terms  are  larger.  These  effects  are 
demonstrated  in  Fig.  4.11.  For  a  step  command  in  position  approximately  twice  the 
size  of  the  previous  one,  the  trajectory  deviates  from  a  straight  line  more  than  the 
previous  one.  This  is  partially  due  to  the  larger  velocities  and  accelerations,  especially 
in  the  middle  of  the  trajectory,  and  partially  due  to  the  saturation  of  the  motors  at  the 
beginning  of  the  trajectory.  It  does  not,  however,  deviate  as  much  from  a  straight  line 
as  the  trajectory  for  the  PD  angle  controller. 

If  the  performance  of  this  controller  is  not  good  enough  to  meet  performance  speci¬ 
fications,  there  are  ways  to  increase  performance.  One  possibility  is  to  use  trajectories 
rather  than  simple  step  commands  in  position.  In  that  way,  acceleration  terms  could 
be  kept  within  set  bounds,  so  they  will  have  less  effect  on  the  trajectory.  Also,  motor 
saturation  can  be  avoided. 

If  this  still  does  not  result  in  the  desired  performance  gain,  an  impedance  controller 
could  be  implemented  using  the  full  equations  of  motion  of  the  manipulator.  This 
controller  algorithm  was  described  by  Hogan  in  [4].  This  would  theoretically  result 
in  a  straight-line  trajectory,  with  only  the  effects  of  unmodelled  dynamics,  errors  in 
modelling,  and  the  digital  rather  than  analog  implementation  of  the  controller  causing 
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Figure  4.11:  Trajectory  for  Impedance  Controller  for  a  Larger  Step 

deviations.  Because  of  the  complexity  of  the  equations  of  motion  of  this  system,  however, 
this  would  be  a  computationally  intensive  process  to  be  performed  inside  a  control  loop. 
The  computational  complexity  might  cause  a  reduction  of  sample  rate  to  the  point  of 
offsetting  any  gains  from  using  the  full  equations  of  motion. 

4.4.2  Force  Control 

The  force  control  experiments  were  performed  with  the  mini-manipulator  tip  already  in 
contact  with  a  massive  target.  The  ability  to  make  a  smooth  touchdown  is  not  tested 
in  any  of  these  experiments. 

The  first  test  of  the  force  controller  was  to  give  a  step  command  increase  to  the 
desired  force  from  0.21V  to  1.01V.  The  force  applied  to  the  target  as  a  function  of  time  is 
shown  in  Fig.  4.12.  In  this  plot  and  all  plots  in  this  section,  the  desired  force  is  shown 
as  a  dashed  line.  The  response  is  well  damped  with  a  very  small  overshoot  and  a  rise 
time  of  less  than  0.14sec.  This  is  a  satisfactory  response  to  a  step  command. 

The  next  experiment  was  the  reverse  of  the  previous.  While  the  force  was  stabilized 
at  1.01V,  the  commanded  force  was  set  to  0.21V.  The  response  should  be  the  inverse  of 
the  previous  one.  This  experiment  was  designed  to  test  whether  a  step  from  a  high  to  a 
low  force  level  would  cause  the  mini-manipulator  tip  to  lose  contact  with  the  target.  The 
response,  which  is  shown  in  Fig.  4.13,  is  similar  to  the  previous  one.  More  importantly, 
the  manipulator  did  not  lose  contact  with  the  target.  Again,  there  is  almost  no  overshoot 
to  the  step  command  in  force. 

Another  experiment  performed  was  to  test  the  response  to  disturbances  in  force. 
The  mini-manipulator  was  held  against  the  surface  with  a  larger  force  than  would  have 
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Figure  4.14:  Response  to  a  Disturbance  in  Force 

been  applied  by  the  motors  alone.  This  force  was  released  and  the  response  recorded. 
This  response  is  shown  in  Fig.  4.14.  It  shows  a  large  swing  in  the  force  below  the 
desired  force  when  the  disturbance  force  is  released.  While  this  behavior  is  expected, 
the  extent  of  it  is  somewhat  distressing.  Had  the  disturbance  force  been  very  much 
larger,  the  mini-manipulator  would  have  lost  contact  with  the  target.  The  gains  should 
be  changed  to  prevent  this  occurence. 

The  final  test  of  the  ability  of  this  controller  to  maintain  a  constant  force  at  a  fixed 
point  on  the  target  was  to  move  the  target  while  the  mini-manipulator  tip  was  in  contact. 
The  target  was  moved  by  hand  with  a  speed  on  the  order  of  Icm/sec.  The  results  are 
shown  in  Fig.  4.15.  This  shows  that  the  errors  in  force  are  all  less  than  10%.  This 
bodes  well  for  the  ability  of  the  mini-manipulator  to  follow  a  moving  target  and  reject 
any  position  disturbances  in  the  target. 

One  more  part  of  the  control  algorithm  should  be  noted.  The  sine  and  cosine  of  the 
angle  at  which  the  force  is  being  applied  is  being  filtered  with  a  single  pole  roll-off  digital 
filter.  This  was  necessary  only  for  the  case  when  the  two  motor  angles  are  nearly  equal 
and  opposite.  Any  significant  noise  in  the  force  sensor  at  this  point  can  cause  the  force 
angle  to  switch  rapidly  between  positive  and  negative.  In  this  part  of  the  workspace, 
this  will  also  cause  the  motor  torques  to  switch  rapidly  between  positive  and  negative.  If 
this  switching  is  too  rapid,  it  exceeds  the  mechanical  time  constant  of  the  motors, which 
will  oscillate  and  not  apply  the  desired  torque. 

In  order  to  have  the  manipulator  maintain  a  desired  force  on  a  fixed  point  on  the 
target,  two  problems  remain  to  be  solved.  First,  as  shown  above,  the  response  to  force 
disturbances  is  currently  unacceptable.  This  problem  can  probably  be  solved  simply 
by  changing  the  gains  on  the  controller.  The  second  problem  is  that  the  manipulator 
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Figure  4.15:  Response  with  moving  target 

tip,  while  at  a  few  positions  in  the  workspace  and  in  contact  with  a  few  different  target 
geometries,  will  not  stay  at  the  contact  point  but  will  drift  a  small  distance  along  the 
surface.  This  problem  has  not  been  solved  as  yet.  It  is  currently  believed  to  be  caused 
by  the  force  sensor  being  slight  misaligned  from  the  vertical,  causing  the  actual  contact 
point  to  be  different  from  the  point  calculated  from  the  mini-manipulator  kinematics. 
This  problem  is  still  being  investigated. 

4.4.3  Surface  Following 

The  control  algorithm  to  have  the  mini- manipulator  tip  follow  the  surface  of  the  target 
while  maintaining  a  constant  force,  described  in  Sect.  4.3.2.2,  was  experimentally  tested 
and  the  results  are  described  in  this  section.  All  the  tests  were  performed  using  a  circular 
target,  the  outline  of  which  is  shown  in  Fig.  4.16.  In  all  cases,  the  commanded  force 
was  1-OiV. 

The  first  test  was  to  command  the  manipulator  tip  to  sweep  along  the  surface  at 
Z.QcmJsec.  The  response  is  shown  in  Fig.  4.17.  This  shows  that  the  force  is  maintained 
at  all  points  on  the  surface  to  within  7%. 

Next,  the  sweep  speed  was  increased  to  13 cm/sec.  The  response  is  shown  in  Fig.  4.18. 
This  shows  that  the  maximum  force  error  has  increased  to  about  13%.  This  increase 
in  force  error  is  expected  since  the  controller  is  now  trying  to  track  variations  in  the 
surface  at  a  faster  rate.  At  even  higher  rates,  the  manipulator  tip  will  lose  contact  with 
the  target. 

This  experiment  was  attempted  at  several  sweep  speeds  to  study  how  the  maximum 
force  error  changed  with  sweep  speed.  The  results  are  shown  in  Fig.4.19.  The  error 
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4.16:  Target  Surface  for  Surface  Following  Experiments 


Figure  4.17:  Surface  Following  at  3.0 cm/ sec 
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Figure  4.18:  Surface  Following  at  13.0 cm/sec 


does  generally  increase  with  sweep  speed  as  expected. 

There  are  currently  two  problems  with  the  performance  of  this  controller.  The  first 
is  that  below  a  certain  commanded  sweep  speed,  the  mini- manipulator  tip  will  stick  at 
one  point  on  the  surface.  While  this  is  expected  at  very  low  rate  due  to  friction  between 
the  force  sensor  and  the  target,  the  sweep  speed  at  which  sticking  currently  appears  is 
unexpectedly  large. 

The  second  problem  is  that  the  performance  appears  to  be  unsymmetric  with  respect 
to  the  direction  in  which  the  target  surface  is  traced.  In  fact,  the  maximum  sweep  speed 
at  which  sticking  occurs  is  approximately  twice  as  large  in  one  direction  as  the  other. 
Also,  the  sweep  speed  at  which  the  mini-manipulator  tip  looses  contact  with  the  surface 
is  approximately  twice  as  large  in  one  direction  as  the  other.  While  the  actual  speeds 
at  which  sticking  and  loss  of  contact  occur  differ  with  different  target  locations,  the 
relation  between  these  quantities  in  the  two  directions  seems  to  be  the  same. 

The  current  explanation  for  these  occurences  is  that  the  force  sensor  probe  is  slightly 
misaligned  from  the  vertical.  Thus  the  position  of  the  manipulator  tip  calculated  using 
kinematics  is  not  precisely  the  same  as  the  actual  position  of  the  tip  of  the  force  sensor. 
The  difference  between  these  will  differ  throughout  the  workspace,  causing  differences 
in  behavior  with  differences  in  position  and  velocity. 


4.4.4  Summary  of  Experimental  Results 

The  response  of  the  mini-manipulator  on  a  fixed  base  in  acceptable,  both  in  position 
and  force  control.  With  the  correction  of  a  few  problems,  these  algorithms  should  be 
ready  for  use  controlling  the  mini-manipulator  on  the  maun  arm. 


4.5.  CURRENT  STATUS  95 


Figure  4.19:  Maximum  Force  Error  as  a  Function  of  Sweep  Speed 

4.5  Current  Status 

Work  with  the  mini-manipulator  on  a  fixed  base  is  nearing  completion.  There  are  still  a 
few  problems  yet  to  be  resolved,  however,  as  described  in  Sects.  4.4.2  and  4.4.3.  Using 
the  mini-manipulator  on  a  fixed  base  has  helped  work  out  many  of  the  problems  that 
would  otherwise  have  been  seen  when  the  mini-manipulator  was  attached  to  the  main 
arm. 

After  completion  of  the  work  on  the  mini- manipulator  on  a  fixed  base,  the  next 
step  experimentally  will  be  to  work  on  control  of  the  main  two-link  arm.  While  the 
actual  algorithms  will  be  the  same  as  those  implemented  by  Hollars,  as  described  in 
Sect.  4.3.2.1,  this  was  implemented  on  a  different  control  computer  with  a  slightly  dif¬ 
ferent  mechanical  configuration.  Therefore,  there  will  be  some  changes  in  the  actual 
coding  of  the  algorithm  required. 

4.6  Further  Research 

The  force  controller  described  in  this  report  uses  force  error,  force  integral  error  and 
velocity  normal  to  the  surface  to  set  the  desired  position  for  the  impedance  controller 
in  a  straightforward  way.  While  this  has  proved  to  be  effective  so  far,  there  are  other 
schemes  that  could  possibly  take  into  account  the  system  dynamics  and  that  would  result 
in  better  performance  of  this  controller.  This  is  one  subject  which  must  be  examined. 

The  control  of  the  main  two-link  arm  to  a  desired  position  should  not  be  very  difficult 
to  implement,  using  the  work  of  Hollars  described  in  Sect.  4.3.2. 1.  However,  there  is  a 
question  of  what  position  should  be  the  desired  position  of  the  main  arm.  It  should  be 
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possible  to  set  the  desired  position  of  the  main  arm  so  that  the  mini-manipulator  is  in 
a  configuration  that  is  “optimal”  in  some  sense.  This  is  a  topic  for  continuing  research. 

Question  about  how  the  effects  of  a  moving  mini-manipulator  base  will  be  included 
in  the  control  algorithm  are  raised  by  the  incorporation  of  the  mini-manipulator  on  the 
tip  of  the  two-link  arm.  It  should  be  possible  to  use  information  about  the  tip  of  the 
main  arm  to  compensate  for  this  motion.  The  exact  method  has  yet  to  be  determined. 

Currently,  the  speed  at  which  the  surface  of  the  target  is  traced  is  a  fixed  quantity, 
specified  by  the  user.  It  would  be  better,  however,  to  set  this  speed  based  on  information 
about  the  surface  such  as  slope  and  curvature.  For  example,  it  would  be  better  to  go 
slowly  when  going  around  a  sharp  corner,  but  the  speed  can  be  much  faster  when 
tracing  a  straight  line.  This  information  can  be  obtained  either  from  previous  tracings 
of  the  same  target  or  on  estimates  based  on  the  surface  just  traversed.  The  method  of 
determining  this  speed  has  not  been  determined. 

The  best  method  for  going  into  and  out  of  contact  with  the  target  has  yet  to  be 
determined.  The  desired  position  for  the  impedance  control  should  be  chose  to  be 
withing  the  surface  such  that  bouncing  does  not  occur  upon  contact.  Also,  the  mini¬ 
manipulator  tip  should  return  to  the  surface  in  the  event  of  loss  of  contact.  The  best 
method  for  doing  this  is  being  looked  into. 


Bibliography 


[1]  Robert  H.  Cannon,  Jr.,  Lawrence  Pfeffer,  Brian  Andersen,  and  Raymond  Kraft. 
DARPA  Annual  Report  1987.  Annual  Report  1,  Stanford  University  Aerospace 
Robotics  Laboratory,  Stanford,  CA  94305,  October  1987. 

[2]  John  J.  Craig.  Introduction  to  Robotics  Mechanics  and  Control.  Addison- Wesley, 
Reading,  MA,  1986. 

[3]  Nevill  Hogan.  Impedance  control:  An  approach  to  manipulation.  7>rjns  of  the 
ASME,  Journal  of  Dyanmic  Systems,  Measurement,  and  Control,  107:1-24,  March 
1985. 

[4]  Nevill  Hogan.  Stable  execution  of  contact  tasks  using  impedance  control.  In  Proceed - 
ings  of  the  International  Conference  on  Robotics  and  Automation,  pages  1047-1053, 
Raleigh,  NC,  April  1987.  IEEE,  IEEE  Computer  Society. 

[5]  Michael  G.  Hollars.  Experiments  in  End-Point  Control  of  Manipulators  with  Elastic 
Drives.  PhD  thesis,  Stanford  University,  Department  of  Aeronautics  and  Astronau¬ 
tics,  Stanford,  CA  94035,  May  1988.  Also  published  as  SUDAAR  568. 

[6]  Lawrence  Pfeffer,  Oussama  Khatib,  and  John  Hake.  Joint  torque  sensory  feedback 
in  the  control  of  a  PUMA  manipulator.  In  Proceedings  of  the  American  Control 
Conference,  pages  818-824,  Seattle,  WA,  June  1986. 


